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ABSTRACT 


A  successful  terrorist  attack  using  a  ship  as  a  weapon  (SAW)  on  shore  infrastructure  in 
the  Malacca  and  Singapore  Straits  would  cause  chaos  to  global  trade,  as  these  Straits 
carry  over  one-quarter  of  the  world’s  commerce  and  half  the  world’s  oil.  This  calamity 
must  be  prevented.  Toward  this  goal,  this  thesis  aims  at  developing  and  detennining  the 
best  distributed  sensor  network  (DSN)  architecture  and  implementing  a  sensor  fusion 
algorithm  for  tracking  a  SAW  intended  to  run  into  the  oil  and  chemical  terminals  on 
Jurong  Island,  Singapore. 

The  work  in  this  thesis  involves  the  application  of  (1)  an  integrated  systems 
engineering  methodology  for  designing  alternative  DSN  architectures,  (2)  Kalman  and 
information  filters  for  SAW  tracking  and  sensor  data  fusion,  (3)  a  track-to-track  fusion 
algorithm,  and  (4)  a  Monte  Carlo  simulative  study  to  assess  the  effectiveness  of  three 
distributed  sensor  fusion  network  architectures — centralized,  de-centralized,  and  hybrid. 
Each  distributed  sensor  fusion  network  architecture  includes  the  various  sensors  that 
Singapore  deploys  in  and  along  the  Singapore  Straits. 

The  simulative  study  results  indicate  that,  with  and  without  communication 
bandwidth  constraints,  a  ship  with  the  intent  to  attack  Jurong  can  be  identified  accurately 
at  an  earlier  time  with  both  the  centralized  and  de-centralized  sensor  fusion  network 
architectures  than  with  the  hybrid  sensor  fusion  network  architecture. 
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I.  INTRODUCTION 


The  lion’s  share  of  Singapore’s  trade  is  conducted  by  sea.  ...  50%  of 
global  oil  shipments,  including  70%  of  Japan’s  oil  imports  and  80%  of 
China's  oil  imports,  pass  through  the  Malacca  Strait  every  day.  As  a 
littoral  state  Singapore  will  continue  to  work  together  with  our 
neighbouring  littoral  states  to  ensure  that  the  Malacca  and  Singapore 
Straits  are  safe  to  allow  freedom  of  navigation  for  global  trade  [1]. 

Minister  for  Defence  Teo  Chee  Hean 
January  16,  2009 

A.  BACKGROUND 

Singapore’s  economy  is  dependent  on  global  trade,  with  the  bulk  of  it  transported 
in  the  maritime  domain.  For  2010,  the  Maritime  Port  Authority  of  Singapore  (MPA) 
estimated  that  Singapore's  container  throughput  was  28.4  million  20-foot-equivalent  units 
(TEUs)  and  total  shipping  tonnage  was  1.9  billion  gross  tons.  Singapore  was  the  world’s 
top  bunkering  port,  with  bunker  sales  reaching  a  record  high  of  40.9  million  tons  [2]. 
While  exact  figures  have  yet  to  be  released  at  the  time  of  writing,  the  maritime 
perfonnance  for  Singapore  is  expected  to  exceed  7%  of  Singapore’s  GDP.  The  straits  of 
Malacca  and  Singapore  are  the  lifelines  of  Singapore,  and  it  is  in  her  vital  interest  to 
ensure  the  undisrupted  flow  of  global  trade  in  these  waterways. 

As  for  global  energy,  the  United  States  Energy  Infonnation  Administration  (U.S. 
EIA)  reports  that  the  straits  of  Malacca  and  Singapore  carry  15  million  barrels  of  oil  per 
day,  making  them  the  world’s  second  most  strategic  chokepoints.  Chokepoints  are 
defined  by  the  U.S.  EIA  as  narrow  channels  along  widely  used  global  sea  routes,  and  they 
pose  navigational  hazards  to  their  heavy  shipping  traffic,  which  can  lead  to  disastrous  oil 
spills  and  exposure  to  piracy  and  terrorist  attacks.  Even  a  temporary  blockage  of  a 
chokepoint  can  lead  to  an  unwelcome  detour,  adding  hundreds  of  miles  to  the  route  and 
substantially  increasing  the  costs  of  freight,  time  delays,  and  even  insurance  premiums 
[3]. 

Piracy  has  plagued  the  straits  of  Malacca  and  Singapore  for  many  years.  Though 
the  number  of  pirate  attacks  has  been  reported  as  on  the  decline  in  recent  years,  with  none 

1 


reported  by  the  International  Maritime  Bureau  (IMB)  [4]  in  the  first  quarter  of  2010  (due 
to  coordinated  military  patrols  among  the  littoral  states  and  user  states),  the  straits  of 
Malacca  and  Singapore  remain  attractive  targets.  Complicating  the  maritime  threat 
picture  is  the  growing  nexus  between  piracy  and  terrorism  [5].  Studies  [6-8]  have 
hypothesized  the  possibility  of  terrorist  attacks  similar  to  those  that  devastated  New  York 
City  on  September  11,  2001,  but  with  ships  as  a  weapon  against  ports,  critical  shore 
installations,  or  other  high-value  ships.  For  example,  in  March  2003,  the  MV  Dewi 
Madrim,  a  chemical  tanker,  was  boarded  by  pirates  while  underway.  Reports  suggested 
that  the  pirates  were  in  fact  terrorists  trying  to  gain  experience  in  ship  handling  before 
conducting  an  attack  using  a  similar  ship  against  U.S.  naval  vessels  in  port  [8],  For  the 
period  of  2002-2008,  Chalk  [5]  reported  a  “modest  yet  highly  discernable  spike  in  high- 
profile  terrorist  attacks  and  plots  at  sea.” 

The  potential  devastation  to  lives  and  the  economy  that  terrorist  attacks  can 
produce  gives  Singapore  a  high  incentive  to  enhance  its  maritime  security.  One  way  to 
enhance  maritime  security  is  to  achieve  maritime-domain  awareness  (MDA),  which  the  U. 
S.  government  defines  as  “the  effective  understanding  of  anything  associated  with  the 
maritime  domain,  all  areas  and  things  of,  on,  under,  relating  to,  adjacent  to,  or  bordering 
on  a  sea,  ocean,  or  other  navigable  waterway”  [9].  In  other  words,  the  key  to  an  effective 
MDA  is  to  make  data  and  infonnation  available  through  a  multitude  of  reliable  sources 
and  high-fidelity  sensors  and  fuse  them  to  provide  clarity  on  a  situation  and  enable  faster 
and  better-infonned  decisions  in  response  to  maritime  threats.  This  requires  systematic 
understanding  and  engineering  of  a  complex  system  of  systems  (SoS)  that  includes 
sensors,  communications  links,  decision-support  systems,  and  engagement  systems  [10]. 

B.  PURPOSE 

The  research  reported  in  this  thesis  seeks  to  identify  the  best  distributed  sensor 
network  (DSN)  architecture  for  a  chosen  sensor  fusion  algorithm  for  Singapore’s 
protection  against  the  ship  as  a  weapon  (SAW). 
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C.  RESEARCH  QUESTIONS 

Two  questions  shape  this  research  and  direct  the  efforts  towards  achieving  the 
purpose  of  this  thesis:  What  sensor-network  architecture  is  best  for  Singapore  to  use  in 
defense  against  a  SAW?  What  is  the  most  suitable  sensor  fusion  algorithm  to  implement 
in  support  of  the  chosen  architecture? 

D.  SCOPE 

The  scope  of  this  research  is  to  develop  a  DSN  architecture  and  implement  a 
sensor  fusion  algorithm  to  realize  accurate  identification  of  SAWs  and  early  warning  of 
impending  impact  upon  Jurong  Island.  The  specific  targets  of  interest  in  this  research  are 
the  oil  and  chemical  terminals  on  Jurong  Island.  This  research  takes  into  consideration 
Singapore’s  existing  suite  of  sensors  (e.g.,  surveillance  and  navigation  radars)  and  sensor 
platforms  (e.g.,  naval  patrol  vessels,  police  and  coast  guard  boats,  maritime  patrol  aircraft, 
and  MPA  radar  towers)  used  in  and  along  the  Singapore  Strait. 

E.  BENEFITS  OF  THE  RESEARCH 

Detennining  and  applying  an  optimal  DSN  architecture  will  produce  optimal 
perfonnance  outcomes  in  terms  of  accurately  identifying  a  ship’s  intentions  and  raising 
the  alarm  in  a  timely  manner.  These  outcomes  will  provide  valuable  insights  in 
developing  response  capabilities  against  SAWs  and  enhancing  maritime  security  in  the 
Singapore  Strait.  The  results  and  concepts  produced  from  this  research  are  intended  to  be 
applicable  to  other  key  installations  within  Singapore  (e.g.,  Tuas  and  Changi  naval  bases 
and  other  commercial  terminals)  and  beyond  (e.g.,  the  Malacca  and  Honnuz  straits),  and 
for  any  given  mix  of  available  sensors  (sea-,  land-,  or  aerial-based  and  fixed  or  mobile). 

F.  APPROACH 

A  three-stage  approach  is  taken  in  this  research.  The  first  stage  develops  different 
sensor  network  architectures.  The  second  stage  identifies  and  implements  a  suitable 
sensor  fusion  algorithm  to  support  these  architectures.  In  the  third  stage,  a  comparative 
study  is  made  using  modelling  and  simulation  to  evaluate  the  performance  of  the  various 
sensor  network  architectures.  The  overall  process  that  guides  these  three  stages  follows 
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closely  the  System-of-Systems  Architecture  Development  Process  (SoSADP)  (Figure  1) 
[11].  The  SoSADP  presents  a  “logical,  systematic,  layered  process”  that  begins  with  a 
SoS  problem  and  flows  through  mission,  needs,  and  requirements  analyses  to  develop 
alternative  SoS  architectures.  It  then  conducts  cost  and  risk  analysis  and  concludes  by 
assessing  and  ranking  the  alternative  architectures  using  modelling  and  simulation.  How 
the  SoSADP  guides  the  development  of  the  various  sensor-network  architectures  for 
MDA  will  be  elaborated  in  later  chapters. 


Figure  1.  System-of-Systems  Architecture  Development  Process  (From:  [11]) 
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G.  THESIS  ORGANIZATION 

This  chapter  provides  the  background,  purpose,  scope,  and  benefits  of  the 
research  and  the  approach  to  achieving  the  stated  goals.  In  Chapter  II,  the  relationships 
among  maritime  domain  security  (MDS),  maritime  domain  awareness  (MDA),  network¬ 
centric  operations  (NCO),  and  data  fusion  (DF)  are  discussed.  Next  is  a  description  of 
the  problem  statement  for  this  research  and  mission,  needs,  and  requirements  analyses, 
which  are  the  first  three  layers  of  the  SoSADP.  In  Chapter  III,  the  fourth  layer  of 
SoSADP,  the  alternative  sensor-network  architectures,  is  developed.  In  Chapter  IV, 
alternative  sensor  algorithms  are  discussed.  In  Chapter  V,  the  fifth  and  last  layer  of 
SoSADP,  a  comparative  study  using  simulation  to  select  the  best  sensor-network 
architecture  and  its  associated  sensor-fusion  algorithm  is  conducted.  Finally,  Chapter  VI 
summarizes  the  research  findings  and  recommends  areas  for  future  work. 
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II.  THE  MARITIME  DOMAIN  SECURITY  PROBLEM 


A.  INTRODUCTION 

The  first  part  of  this  chapter  describes  the  security  challenges  posed  in  the 
maritime  domain,  rationalizes  the  need  for  maritime  domain  awareness,  explains  how 
network-centric  operations  (NCO)  support  the  enhancement  of  MDA,  and  discusses  the 
relationship  between  data  fusion  (DF)  and  NCO.  The  second  part  of  this  chapter 
describes  the  research  problem  and  carries  out  the  first  three  layers  of  the  SoSADP:  the 
analysis  of  mission,  needs,  and  requirements. 

1.  Maritime  Domain  Security  (MDS) 

Acts  of  terrorism  remain  a  clear  and  present  danger  today  and  for  many  years  to 
come.  Studies  [5,  8]  have  predicted  that  terrorist  acts  in  the  maritime  domain  are  likely  to 
continue,  if  not  grow.  Five  reasons  for  the  shift  in  terrorist  attack  from  predominately 
land-  to  sea-based  are  put  forth  by  Chalk  [5].  First,  the  same  openness  and  lack  of 
regulation  in  many  port  and  coastal  areas  that  encourages  pirate  attacks  applies  equally  to 
acts  of  terror.  Second,  the  accessibility  and  growth  of  maritime  sports  and  activities  have 
granted  training  and  resource  opportunities  for  operations  at  sea.  The  financial  and 
psychological  devastation,  in  the  short  and  long  terms,  to  local  and  global  economies  that 
can  be  achieved  from  a  successful  and  sizeable  maritime  attack  is  very  attractive  account 
for  the  other  two  reasons.  Finally,  extensive  and  hard-to-inspect,  global  container 
shipping  facilitates  the  covert  movement  of  resources  (arms  and  explosives,  chemical  and 
biological  material,  nuclear  material,  etc.).  Therefore,  in  order  to  thwart  a  terrorist  attack 
in  the  maritime  domain,  one  would  need  to  build  barriers  to  harden  security  and  not  let 
these  five  factors  become  an  option  of  choice. 

2.  Maritime  Domain  Awareness  (MDA) 

One  way  to  enhance  MDS  is  to  achieve  MDA.  In  Chapter  I,  MDA  is  defined  as 
knowing  everything  and  anything  that  happens  in  the  maritime  domain.  Therefore, 
achieving  MDA  rests  on  the  ability  to  continuously  survey  maritime  and  maritime-related 
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activities  such  that  contacts  of  interest  can  be  detected,  their  movements  tracked,  and 
their  intention  classified.  Galdorisi  and  Goshom  [12]  list  seven  MDA  requirements  that 
are  imperative  to  attaining  a  comprehensive  and  accurate  appreciation  of  maritime 
domain  activities: 

•  Focused  sensing  and  data  acquisition 

•  Dynamic,  interoperable  connectivity 

•  Responsive  information  management 

•  Infonnation  assurance 

•  Consistent  representation 

•  Distributed  collaboration 

•  Dynamic  decision  support 

While  these  seven  requirements  represent  vitally  important  parts  of  the  “MDA 
value  chain,”  the  last  two  bear  most  directly  on  the  challenge  of  comprehensive  MDA. 
Focused  sensing  and  data  acquisition  refers  to  the  collection  of  specific  data  and 
information  on  the  contact  or  area  of  interest,  as  opposed  to  gathering  a  barrage  of 
ambiguous  data.  Dynamic  decision  support  emphasizes  the  need  to  deploy  sensing 
resources  efficiently  and  sparingly,  as  more  data  does  not  mean  better  data.  In  this  thesis, 
these  two  MDA  requirements  are  considered  in  the  evaluation  of  sensor-network 
architecture  and  data  fusion  algorithms.  The  remaining  five  requirements  form  the 
elements  of  NCO.  Though  not  the  primary  focus  of  this  thesis,  they  are  indispensible  in 
the  attainment  of  comprehensive  MDA  and  are  briefly  discussed  next. 

3.  Network-Centric  Operations  (NCO) 

The  cornerstone  of  NCO  lies  with  a  networked  force.  Networks  can  increase 
operational  effectiveness,  because  they  enable  distributed  sensors  to  be  synchronized  in 
time,  facilitate  communication  among  dispersed  forces,  and  integrate  intelligence, 
surveillance,  and  reconnaissance  to  build  and  share  a  common  operating  picture  [13]. 
The  five  NCO-related  MDA  requirements  are  now  elaborated. 

•  Dynamic,  interoperable  connectivity.  The  expansiveness  of  the  maritime 
domain  demands  that  all  users  have  reliable,  secure,  and  flexible  access  to 
each  other  and  other  infonnation  sources.  Connectivity  must  be  dynamic 
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in  order  to  address  the  changing  real-time  needs  of  users  and  changes  to 
the  environment  as  bandwidth  demands  change  along  with  the  operation 
scenario. 

•  Responsive  infonnation  management.  The  user  must  have  enough 
information  to  make  informed  decisions,  but  not  so  much  as  to  cause 
information  overload.  Hence,  the  methods  of  accessing  infonnation  — 
user  information  pull,  producer  infonnation  push,  and  planned  information 
ordering  —  need  to  be  well  balanced. 

•  Infonnation  assurance.  The  provision  of  access  controls,  authentication 
mechanisms,  confidentiality,  and  integrity  features  enables  users  to  assert 
their  identity  and  access  resources  in  both  peer-peer  and  client-server 
interactions. 

•  Consistent  representation.  Infonnation  must  be  consistent  spatially, 
temporally,  and  as  to  content  for  users  to  act  in  sync.  While  every  user  at 
every  level  is  not  necessarily  required  to  view  the  identical  common 
operational  picture  at  all  times,  every  user  must  have  access  to  the  same 
accurate  and  timely  information. 

•  Distributed  collaboration.  This  imperative  involves  maintaining  fully 
connected  and  transparent  interactions  among  users  and  providing  tools 
and  connectivity  for  collaboration  at  the  user  level. 

MDA  cannot  be  achieved  effectively  without  networks,  and  networks  cannot  be 
evaluated  in  isolation  without  considering  limitations  or  disruptions  to  communications. 
In  this  thesis,  communications  bandwidth  is  used  as  a  limiting  factor  to  detennine  its 
interaction  and  influence  on  the  performance  of  the  sensor  network  architectures  studied. 

4.  Data  Fusion 

Chamiak  et  al.  describes  data  fusion  (DF)  as  “a  multilevel,  multifaceted  process 
dealing  with  the  detection,  association,  correlation,  estimation  and  combination  of  data 
and  infonnation  from  multiple  sources  to  achieve  refined  state  and  identity  estimation, 
and  complete  and  timely  assessments  of  situation  and  threat”  [14].  The  output  produced 
from  fusing  data  captured  from  a  multitude  of  differing  sensors  should  be  richer  in 
infonnation  than  any  individual  sensor  can.  Furthennore,  the  automation  of  the  DF 
process  is  necessary  today  because  the  vast  amount  of  data  captured,  along  with 
progressively  more  complex  and  ambiguous  data  measurements,  is  exceeding  the  human 
ability  to  associate  and  classify  manually.  Thus,  there  is  a  push  for  sensor  networks  to 
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incorporate  automation  “of  all  or  part  of  the  individual  processes  of  fusion,  to  provide 
timely,  accurate,  and  easily  understood  infonnation,  rather  than  masses  of  raw  data”  [15]. 
Parenthetically,  in  this  thesis,  the  terms  DF  process  and  DF  will  be  used  interchangeably. 

Effective  data  fusion  is  dependent  on  the  underlying  sensor-network  architecture, 
fusion  algorithm,  and  communication  links  within  the  network  [16].  In  this  thesis,  the 
NCO  provides  the  context  on  how  data  fusion  should  be  employed  to  fuse  data  captured 
independently  from  different  sensor  systems  from  different  agencies.  Communications 
bandwidth  limits  are  taken  into  account  to  detennine  whether  DF  should  be  centralized  or 
distributed.  This  in  turn  determines  if  sensors  or  sensor  nodes  should  transmit  large 
quantities  of  raw  data  or  processed  track  estimates.  Transmitting  raw  data  may  ensure 
perfonnance  optimality  by  creating  “super”  measurements,  but  may  not  be  practical 
under  realistic  operational  settings.  In  addition,  while  transmitting  processed  track 
estimates  can  reduce  bandwidth  requirements,  it  adds  more  processing  layers  and  may 
lose  some  richness  of  infonnation. 

B.  PROBLEM  STATEMENT 

The  distributed  sensor  network  architecture  problem  statement  is  a  description  of 
the  DSN  that  needs  to  be  built,  and  it  sets  in  motion  the  SoSADP.  It  provides  missions, 
threats,  multiagency  undertaking,  timeframe,  geographical  settings,  needs  in  a  sensor 
network,  and  constraints  [11]. 

In  this  research,  the  problem  to  be  addressed  is  captured  in  two  questions:  what 
should  the  DSN  architecture  be,  given  the  different  and  dispersed  sensors  and  sensor 
platforms  used  by  Singapore,  and  what  is  the  most  suitable  sensor  fusion  algorithm  to 
support  the  architecture  so  as  to  enable  accurate  and  early  identification  of  hostile  intent 
by  a  ship  transiting  the  Singapore  Strait?  Singapore  and  the  Singapore  Strait  serve  as  the 
setting  for  this  research.  The  SAW  is  assumed  to  attack  the  chemical  and  oil  tenninals  on 
Jurong  Island. 
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C.  MISSION  ANALYSIS 

Mission  analysis  consists  of  determining  threats,  refining  missions,  defining 
scenarios,  and  detennining  mission  requirements  [11].  A  scenario  includes  the  threats 
and  their  possible  navigation  routes,  the  targets  and  their  locations,  the  sensors,  and  the 
physical  environment  they  all  operate  in. 

1.  Operating  Environment 

a.  Physical  Characteristics 

The  Singapore  Strait  is  located  south  of  both  the  island  of  Singapore  and 
the  southeastern  tip  of  peninsular  Malaysia,  north  of  the  Indonesian  Rian  Islands.  The 
narrowest  breadth  of  the  Singapore  Strait  is  only  about  three  miles,  and  throughout,  it  is 
less  than  15  miles  wide.  At  its  eastern  outlet  into  the  South  China  Sea,  where  it  is 
bounded  solely  by  Malaysia  and  Indonesia,  the  seaway  is  approximately  1 1  miles  wide 
[17].  The  traffic  separation  scheme  (TSS)  adopted  by  the  International  Maritime 
Organization  (IMO)  in  1977  further  constrains  the  allowable  seaway  for  navigation.  The 
TSS  divides  the  straits  of  Malacca  and  Singapore  into  nine  sectors  with  Sectors  1  to  6 
lying  in  the  Malacca  Strait  and  Sectors  7  to  9  in  the  Singapore  Strait  (Figure  2).  To 
provide  an  appreciation  of  the  restrictedness  of  navigating  in  the  Singapore  Strait,  Table  1 
tabulates  the  narrow  points  within  the  Strait,  with  the  narrowest  just  0.33  nautical  miles  in 
Sector  8,  south  of  St.  John’s  Island. 

b.  Shipping  Traffic 

As  a  consequence  of  high  shipping-trade  volume  in  a  narrow  strait,  ship 
traffic  density  in  the  Singapore  Strait  is  generally  high  and  especially  so  in  the  Philip 
Channel  [18].  By  regulation,  very  large  crude  carriers  (i.e.,  a  tanker  of  150,000  DWT 
and  above)  and  deep-draught  vessels  (45  feet  and  above)  navigating  in  the  Strait  are 
prohibited  from  traveling  at  speeds  exceeding  12  knots  [19].  Otherwise,  vessel  speeds 
have  been  reported  to  range  from  8  to  25  knots  [18].  For  navigational  safety,  the  general 
traffic  flow  is  about  15  knots. 
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Figure  2.  Traffic  Separation  Scheme  in  the  Singapore  Strait  (From:  [20]) 


Table  1.  Summary  of  Narrow  Points  in  the  Traffic  Separation  Scheme 

(After:  [20]) 


Sectors  7-9 
(Singapore  Strait) 

Overall 

1.34  nautical  miles 

(near  St  John’s  Island,  Singapore,  Sector  8) 

Eastbound 

1  nautical  mile 

(near  St  John’s  Island,  Singapore,  Sector  8) 

Westbound 

0.33  nautical  miles 

(near  St  John’s  Island,  Singapore,  Sector  8) 
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c. 


Maritime  Conditions 


The  tidal  range  in  the  Singapore  Strait  varies  within  5  feet  and  the  tidal 
streams  reach  up  to  6  knots  [7].  Ships  with  deep  draughts  may  be  concerned  with  tidal 
heights,  but  the  tidal  stream  would  have  little  effect  on  ships  with  shallow  draughts,  such 
as  a  very  large  crude  carrier  traversing  at  high  speed.  In  other  words,  the  tidal  conditions 
pose  a  minimal  barrier  to  a  SAW  of  large  size  travelling  at  high  speeds  towards  its  target. 
Furthennore,  the  sea  is  usually  calm,  with  a  sea-state  condition  of  one  or  at  most  two. 
Visibility  in  general  is  good  over  the  whole  area,  except  during  heavy  rain  showers  [21]. 

d.  Climate  Conditions 

The  climate  of  the  Straits  of  Singapore  is  equatorial,  with  unifonn  high 
temperature,  high  humidity,  and  copious  rainfall.  Prevailing  winds  are  generally  north  to 
northeast  during  the  Northeast  Monsoon  (December  to  April)  and  south  to  southeast 
during  the  Southwest  Monsoon  (June  to  October).  Wind  speeds  average  less  than  10 
knots  throughout  the  year,  although  occasional  strong  gusts  are  likely  when  showers  or 
thunderstorms  are  in  the  vicinity.  In  sum,  the  climate  conditions  in  the  Singapore  Straits 
are  fairly  constant  and  would  not  pose  a  navigational  deterrent  to  using  a  ship  as  a 
weapon. 

2.  Potential  Targets 

The  southern  coast  of  Singapore  presents  an  attractive  target  for  seaborne  terrorist 
attacks  because  of  its  accessibility  and  proximity  to  the  Straits  of  Singapore.  There  are 
several  potential  targets  along  the  southern  coast — seaports,  industrial  parks,  Sentosa 
Island,  and  even  naval  bases — but  Jurong  Island  stands  out  among  them  all.  Jurong  plays 
a  significant  role  in  Singapore’s  economy  and  the  global  business  ecosystem.  It  hosts 
over  95  global  companies,  including  heavyweights  such  as  Shell,  ExxonMobil,  Chevron, 
DuPont,  BASF,  Sumitomo  Chemicals,  and  Mitsui  Chemicals,  which  collectively  have 
invested  more  than  S$31  billion  in  fixed  assets  on  the  island.  In  addition,  these 
companies  are  the  largest  contributors  to  Singapore’s  manufacturing  output,  at  almost 
40%  in  2008  [22-23].  And  because  a  successful  terrorist  attack  would  trigger  undesired 
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large-scale  economic,  energy,  and  environmental  destruction,  Jurong  Island  was  included 
in  the  list  of  restricted  areas  on  8  December  2006  [25]  to  guard  against  approach  by 
unauthorized  vessels.  Hence,  Jurong  Island  is  the  ideal  target  choice  for  this  research. 

Three  locations,  Al,  A2,  and  A3  (Figure  3),  have  been  identified  as  potential 
impact  destinations  for  a  SAW.  They  are  selected  primarily  due  to  their  proximity  to  the 
Singapore  Strait  and  high  concentration  of  refineries  and  infrastructure  near  the  coast. 
Furthermore,  any  sort  of  attack  against  any  of  these  three  areas  would  be  equally 
applicable  to  the  adjacent  areas. 


Figure  3.  Jurong  Island,  Located  Southwest  of  Singapore  Mainland  (From:  [24]) 
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Figure  4.  Identified  Target  Areas  of  Interest  in  Jurong  Island  (From:  [25-26]) 
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3. 


Threat  Characteristics 


Singapore  is  ranked  as  one  of  the  world’s  busiest  port,  with  an  average  annual 
arrival  of  about  130,000  vessels  of  75  gross  registered  tons  (GRT)  and  above  [27].  While 
it  has  been  reported  that  some  types  of  ship  (particularly  smaller,  general-cargo  ships, 
product  tankers,  tugs,  and  barges)  are  more  likely  to  be  attacked  than  others  [17],  the 
report  is  based  mostly  on  piracy  and  sea-robbery  incidents.  A  survey  of  high-profile 
maritime  terrorist  incidents  from  1961  to  2004  shows  that  the  targets  of  terrorist  attacks 
consisted  of  cruise  ships,  tankers,  freighters,  coasters,  trawlers,  small  boats,  ports,  and 
offshore  oil  tenninals  [5],  In  this  research,  it  is  deemed  that  a  SAW  must  be  able  to 
overcome  offshore  barriers  such  as  water  barricades  and  wharves.  On  impact,  the  SAW 
must  not  only  create  significant  direct  damage  but  also  cause  extensive  collateral  damage 
to  onshore  structures  such  as  oil  refinery  plants.  Hence,  vessels  that  are  both  large  and 
fast  (i.e.,  traversing  at  a  speed  of  greater  than  20  knots),  such  as  containers  and  tankers, 
are  potential  threats  of  concern.  In  2010,  the  count  of  40,322  containers  and  tankers 
represented  about  31%  of  total  shipping  arrivals  in  Singapore  (Table  2).  This  is  a  sizable 
number  and  such  vessels  are  a  recognized  threat. 


Table  2.  Numbers  and  Types  of  Vessel  Arrivals,  Over  75GRT,  to  Singapore 

(From:  [27]) 


Year 

Total 

Containers 

Freighters 

Coasters 

Bulk 

C  arriers 

Tankers 

Regional 

Ferries 

Barges 

Tugs 

Misc. 

2002 

142,745 

16,418 

4,867 

5,869 

5,097 

16,919 

m 

mm 

13,704 

9,902 

2003 

135,386 

16,155 

4,477 

6,091 

5,298 

17,084 

12,597 

9,414 

2004 

133,185 

4,386 

5,897 

5,327 

17,576 

13,947 

12,274 

9,314 

2005 

130,318 

4,594 

5,228 

6,636 

17,315 

12,904 

11,853 

9,871 

2006 

128,922 

19,161 

4,610 

4,909 

7,912 

18,195 

37,986 

12,789 

12,561 

10,009 

2007 

128,568 

19,946 

4,873 

4,991 

8,653 

19,312 

36,530 

11,600 

11,772 

10,160 

2008 

131,695 

20,589 

5,083 

4,619 

9,280 

19,460 

32,643 

14,047 

13,736 

11,215 

2009 

130,575 

18,005 

4,415 

3,850 

11,873 

20,080 

31,247 

14,778 

15,269 

10,129 

2010 

127,299 

18,967 

4,619 

4,123 

12,234 

21,355 

31,094 

11,972 

12,551 

9,739 
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a. 


Characteristics  of  Containers 


Container  vessels  represent  about  11.6%  of  the  worldwide  merchant-fleet 
tonnage  and  they  make  up  14.6%  of  vessel  arrivals  in  Singapore.  They  range  in  size  from 
small  vessels  of  about  1,200  deadweight  tonnage  (DWT)  to  very  large  ships  lifting  up  to 
about  12,000  TEUs.  Speeds  range  from  about  14  knots  for  the  smaller  vessels  to  25  knots 
for  the  larger  ships  [18].  The  possibility  of  containers  carrying  nuclear  devices  or 
weapons  of  mass  destruction  (WMD)  is  of  growing  concern  to  maritime-safety  agencies, 
and  more  so  after  several  spectacular  accidents  due  to  the  combustion  or  explosion  of 
undeclared  or  mis-declared  cargo  [28]. 

b.  Characteristics  of  Tankers 

Tankers  make  up  16.4%  of  vessel  arrivals  in  Singapore.  There  are  several 
variants,  mainly  oil,  chemical,  and  liquefied  gas.  Crude-oil  tankers  are  generally  large 
vessels  over  100,000  DWT  with  speeds  of  around  15  knots.  A  successful  terrorist  attack 
on  a  crude-oil  tanker  could  cause  massive  economic  and  environmental  damage. 
Chemical  tankers,  by  contrast,  are  smaller,  ranging  from  about  1,000  DWT  to  50,000 
DWT,  with  speeds  of  around  10  to  12  knots  for  the  smaller  vessels.  They  carry  highly 
volatile  cargoes  that,  in  the  event  of  a  spill  or  marine  accident,  may  pose  severe  hazards 
to  human  life,  property,  and  the  marine  environment.  The  liquefied-gas  tankers  range 
widely  in  size,  from  small  liquefied-petroleum-gas  carriers  of  about  1,000  DWT  to  large 
liquefied-natural-gas  carriers  of  around  90,000  DWT.  The  larger  vessels  are  relatively 
fast,  with  service  speeds  of  about  18  knots.  The  cargo  carried  by  these  ships  can 
potentially  be  used  as  a  floating  bomb  [29],  and  hence,  are  a  potential  target  for  terrorist 
attacks. 


4.  Attack  Approaches 

As  a  SAW,  intending  to  attack  Jurong  Island,  is  traversing  the  Singapore  Strait 
within  the  TSS,  it  makes  an  abrupt  turn  towards  the  island  when  it  reaches  the  narrowest 
distance  from  the  TSS  to  the  island.  Figures  5  and  6  illustrate  the  different  attack 
approaches  the  SAW  can  take. 
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Examining  the  TSS  and  the  distances  from  the  TSS  to  Jurong  Island,  the  least 
noticeable  approach  to  Jurong  would  be  to  transit  east  to  west  then  turn  towards  area  A1 
(see  the  red,  bold  lines  in  Figure  5).  This  approach  offers  the  shortest  distance  from  the 
point  where  the  SAW  leaves  the  TSS  for  area  Al,  giving  authorities  the  least  time  to 
assess  intent  and  to  respond.  It  is  very  likely  that  this  same  route  will  be  taken  to  reach 
areas  A2  and  A3,  because  it  affords  a  shorter  distance  as  compared  to  turning  in  at  the 
south  of  Sentosa,  where  shipping  traffic  is  probably  higher  due  to  the  presence  of  Sentosa, 
Keppel  Terminal,  and  Pasir  Panjang  Terminal  (see  the  black,  dashed  lines  in  Figure  5). 

If  the  SAW  approaches  Jurong  Island  while  traversing  west  to  east,  it  will  attract 
considerable  attention  when  it  cuts  across  the  TSS  before  heading  towards  Jurong  Island 
(see  the  black,  dashed  lines  in  Figure  6).  It  is,  therefore,  deemed  unlikely  that  the  SAW 
will  take  a  west-to-east  approach. 

In  the  worst-case  scenario,  the  SAW  would  take  the  westbound  route,  navigate  the 
TSS  without  attracting  much  attention  and  launch  an  attack  on  area  Al.  This  attack 
approach  offers  the  SAW  the  shortest  travel  from  the  TSS. 
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Figure  5.  The  East-to-West  Routes  Likely  to  Be  Taken  by  a  SAW  to  Attack  Jurong 

Island  (After:  [24]) 


Figure  6.  The  West-to-East  Routes  Likely  to  Be  Taken  by  a  SAW  to  Attack  Jurong 

Island  (After:  [20]) 
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5.  Sensors,  Sensor  Platforms,  and  Fusion  Centers 

This  section  describes  the  organic  sensors,  sensor  platforms,  and  fusion  centers 
used  by  Singapore  to  achieve  MDA  in  the  Singapore  Strait.  These  sensing  and  data- 
processing  resources  are  used  in  this  thesis  as  elements  of  sensor  network  architectures. 
A  brief  description  of  the  use  of  open-source  shipping  databases  and  some  future  sensor 
platforms  is  included  here  for  completeness. 

a.  Maritime  Port  Authority  of  Singapore 

The  Maritime  and  Port  Authority  of  Singapore  (MPA)  [30]  is  responsible 
for  the  safe  navigation  of  ships  within  port  waters  and  the  Strait.  It  closely  monitors 
vessel  movements  in  these  waters  through  two  port  operations  control  centers  (POCCs), 
located  at  Tanjong  Pagar  Complex  (POCC1)  and  PSA  Vista  (POCC2).  POCC1  monitors 
vessel  traffic  in  eastern  port  waters,  while  POCC2  is  responsible  for  traffic  in  the  western 
sector  and  Singapore  Strait. 

Both  centers  employ  the  multi-radar  Vessel  Traffic  Infonnation  System 
(VTIS)  to  track  and  monitor  shipping  of  up  to  5,000  vessels  in  real-time  in  the  Strait  and 
port  waters.  Under  an  International  Maritime  Organization  (IMO  approved,  mandatory 
ship-reporting  system  for  the  Malacca  and  Singapore  straits  (STRAITREP),  ships  larger 
than  300  gross  tons  and  all  passenger  vessels  must  report  to  the  coastal  Vessel  Traffic 
Service  (VTS)  authorities  when  approaching  Horsburgh  Lighthouse  in  the  east  or  Tg  Piai 
(southwest  of  Johor)  in  the  west.  Having  received  the  vessels’  reports,  the  VTIS  starts 
continuous  monitoring  of  their  movements  into  port. 

The  MPA  has  also  set  up  automatic  identification  systems  (AIS), 
transponder  base  stations  to  enable  its  control  centers  to  automatically  receive  ship 
identities  and  positions  transmitted  from  transponders  carried  aboard.  These  base  stations 
are  located  along  the  southern  coast  of  Singapore  to  cover  the  Singapore  Strait  and  port 
waters.  For  small  vessels  that  are  not  required  to  carry  AIS  transponders,  the  MPA  and 
enforcement  agencies  have  developed  and  implemented  a  harbor-craft  transponder 
system  (HARTS)  to  give  small  vessels  the  ability  to  transmit  craft  identity,  position, 
course,  and  speed. 
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b. 


Fearless  Class  Patrol  Vessels 


Republic  of  Singapore  Navy  (RSN)  patrol  vessels  (PVs)  (Figure  7)  patrol  the 
Singapore  Strait  daily  to  enhance  the  coastal  security  of  Singapore. 


Figure  7.  Photograph  of  a  Patrol  Vessel  (From:  [31]) 


The  sensors  [32]  a  PV  carries  include: 

(1)  Radars 

•  Surface  search  and  fire  control:  Elta  EL/M- 

2228(X);  I-band 

•  Navigation:  Kelvin  Hughes  1007;  I-band 

(2)  Electro-Optical 

•  Elbit  MSIS  optronic  director 

(3)  Sonar 

•  Hull-mounted  Thomson  Sintra  TSM  2362 

Gudgeon;  active  attack;  medium  frequency 

(4)  Electronic  countenneasures 

•  Electronic  support  measures:  Elisra  NS-9010C; 
intercept 
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c.  Coastal  Patrol  Craft 

The  Police  Coast  Guard  (PCG)  (Figure  8)  enforces  the  law,  maintains 
order  in  Singaporean  territorial  waters,  and  acts  to  prevent  and  detect  crime.  Of  interest 
is  the  coastal-patrol  squadron,  which  operates  the  newly  replaced  PFI-class  coastal-patrol 
craft  (CPC)  to  secure  the  sea  passages  in  Singaporean  waters  and  ensure  safe  passage  for 
legitimate  users.  The  new  boats  may  be  equipped  with  enhanced  radar  and  electro- 
optical  surveillance  systems  [33]. 


Figure  8.  Photograph  of  a  Coastal  Patrol  Craft  (From:  [34]) 
d.  Maritime  Patrol  Aircraft 

The  F50  Maritime  Patrol  Aircraft  (MPaA)  (Figure  9)  is  jointly  operated  by 
the  RSN  and  Republic  of  Singapore  Air  Force  (RSAF).  Its  mission  is  to  provide 
maritime  air  surveillance  to  protect  the  seaward  defense  and  sea  lines  of  communication. 
[31].  Together  with  counterparts  from  Malaysia,  Indonesia,  and  Thailand,  the  F50 
routinely  conducts  sorties  over  the  Singapore  Strait  and  is  currently  involved  in  “eyes  in 
the  sky”  joint  maritime  air  surveillance  of  the  Malacca  Straits.  A  maritime-patrol  aircraft 
would  be  equipped  with,  presumably,  surveillance  radar  and  an  electro-optical  sensor. 
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Figure  9.  Photograph  of  a  Maritime  Patrol  Aircraft  (From:  [31]) 

e.  Fusion  Centers 

Depending  on  the  DSN  architecture,  there  can  be  one  or  many  data-fusion 
centers  within  a  network,  as  discussed  in  the  next  chapter.  At  this  juncture,  it  suffices  to 
know  that  each  maritime  agency  (i.e.,  MPA,  RSN,  RSAF,  or  PCG)  has  its  own  data- 
fusion  center.  For  MPA,  the  fusion  center  is  the  POCCs,  while  for  the  RSN,  RSAF,  and 
PCG,  fusion  centers  are  assumed  to  reside  in  their  respective  headquarters  (HQs).  Over 
and  above  these  fusion  centers,  the  Singapore  Maritime  Security  Center  (SMSC)  was  set 
up  in  2009  to  advance  interagency  cooperation  among  Singapore’s  maritime  agencies. 

f  Open-Source  Databases 

Various  databases,  both  commercial  and  open  source,  contain  voluminous 
information  that  could  aid  in  profiling,  identifying,  and  preempting  a  SAW.  Some  of 
these  databases  include  Lloyd’s  List  Intelligence,  the  International  Chamber  of 
Commerce,  AISLive,  and  MaritimeTraffic. 
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Future  Sensors 


g- 

Autonomous  aerial,  surface,  and  underwater  vehicles,  equipped  with  radar, 
sonar,  and  electro-optical  sensors,  can  be  deployed  to  patrol  the  Singapore  Strait  and 
augment  current  maritime-security  resources.  While  having  more  sensors  presumably 
helps  increase  MDA,  data  overload  or  excessive  redundancy  can  lead  to  wastage. 
Furthermore,  prudence  refrains  from  adding  more  “vehicles”  to  the  already  congested  and 
shallow  waters  of  the  Singapore  Strait.  Space  surveillance  via  satellite  is  another  area  to 
be  explored.  While  satellite  offers  a  much  bigger  surveillance  footprint,  using  it  solely 
for  the  monitoring  the  Singapore  Strait  may  not  be  a  cost-effective  option.  These  sensors 
are  mentioned  for  awareness  purposes  and  will  not  be  discussed  or  used  further  in  the 
remainder  of  this  thesis. 

6.  Sensor  Coverage 

A  sensor-coverage  analysis  is  conducted  to  determine  the  level  of  sensor  coverage 
effected  by  the  sensors  and  sensor  platforms  used  in  this  research.  The  level  of  sensor 
coverage  is  assessed  in  terms  of  time,  space,  and  the  number  of  sensors  involved.  The 
TSS  in  the  Singapore  Strait  is  subdivided  into  10  sectors  (Figure  10)  and,  for  a  typical 
day,  each  sensor  platform  is  assumed  to  undertake  a  predetennined  sector-patrol  profde 
in  the  Strait.  The  detection  ranges  of  the  sensors  used  are  adopted  from  open  literature 
and  are  typical  for  the  operating  environment  in  the  Singapore  Strait.  A  Monte-Carlo 
simulation  of  50  runs  over  30  days  is  carried  out.  To  add  realism  to  the  sensor  and 
sensor-platfonn  operating  profdes,  sensor  failures  (assumed  to  be  one  failure  per  day, 
with  a  turnaround  of  two  hours)  are  factored  into  the  simulation.  Details  of  the  sensor- 
coverage  simulation  are  in  Appendix  A. 

The  results  of  the  sensor  coverage  simulation  are  illustrated  in  Figure  1 1 .  Jurong 
Island  lies  in  Sector  73.  As  can  be  seen,  the  minimum  and  the  maximum  numbers  of 
available  sensors  in  that  sector  are  three  and  six,  respectively.  These  figures  will  then  be 
used  in  the  simulative  study  in  Chapter  V  for  sensor  network  architecture  performance 
evaluation. 
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Legend:  _ 
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Figure  10.  An  Instance  of  the  Sensor  Platfonn’s  Disposition  and  Sensor  Coverage 

Areas  (After:  [35]) 


Figure  1 1 .  Average  Number  of  Operational  Sensors  in  Each  Sector  of  the  Singapore 

Strait  in  a  Day 
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D.  NEEDS  ANALYSIS 


The  needs  analysis  layer  is  built  upon  the  output  of  ‘Define  Missions’  in  the 
mission-analysis  layer  of  the  SoSADP  (Figure  1).  The  purpose  of  the  needs  analysis  is  to 
ascertain  which  functions  the  sensor  network  must  perform  and  to  develop  measures  of 
effectiveness  (MOEs)  for  the  level  of  perfonnance  the  sensor  network  must  attain  to 
support  the  mission. 

1.  Sensor-Network  Needs 

The  mission  of  the  sensor  network  in  this  research  is  to  provide  early  and  accurate 
identification  of  a  SAW  launching  an  attack  on  Jurong  Island.  The  mission  is  considered 
achieved  if  the  SAW  is  identified  before  reaching  its  intended  destination  (i.e.,  Jurong 
Island).  The  interdiction  of  a  SAW  is  beyond  the  scope  of  this  research,  but  the  outcome 
of  this  research  would  provide  valuable  insights  for  the  design  of  the  response  against  the 
SAW. 


2.  Measures  of  Merit 

The  relationships  between  DF  performance  and  military  effectiveness  must  be 
properly  understood  in  order  to  develop  measures  and  models  that  relate  them.  The 
Military  Operations  Research  Society  [36]  has  recommended  a  hierarchy  of  measures 
that  relate  perfonnance  characteristics  of  command,  control,  and  communications  (C3) 
systems  (including  fusion)  to  military  effectiveness  (Table  3).  Their  recommendations 
provide  a  quantified  measure  of  merit  on  how  improved  information  accuracy,  timeliness, 
and  content,  enabled  by  sensors  and  fusion  processes,  maps  to  military  effectiveness. 
MOFEs,  MOPs,  and  dimensional  parameters  are  not  used  in  this  work,  but  their 
definitions  are  included  for  completeness. 
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Table  3.  Four  Categories  of  Measures  of  Merit  (From:  [36]) 


Measure 

Definition 

Typical  Examples 

Measures  of 
force 

effectiveness 

(MOFEs) 

Measure  of  how  a  C3  system  and 
the  force  of  which  it  is  a  part 
(sensors,  weapons,  C3  system) 
perform  military  missions 

Outcome  of  battle;  cost  of 
system;  survivability;  attrition 
rate;  exchange  ratio;  weapons  on 
targets 

Measures  of 
effectiveness 
(MOEs) 

Measure  of  how  a  C3  system 
functions  within  an  operational 
environment 

Target  nomination  rate; 
timeliness  of  information; 
accuracy  of  information;  warning 
time;  target  leakage; 
countermeasure  immunity; 
communications  survivability 

Measures  of 
perfonnance 
(MOPs) 

Measures  closely  related  to 
dimensional  parameters  (both 
physical  and  structural),  but 
measure  attributes  of  behaviour 

Detection  probability;  false- 
alarm  rate;  location  transmission; 
communication  delay;  sensor 

Dimensional 

parameters 

The  properties  or  characteristics 
inherent  in  the  physical  entities 
whose  values  detennine  system 
behaviour  and  the  structure  under 
question,  even  when  not  operating 

Signal-to-noise  ratio;  operations 
per  second;  aperture  dimensions; 
bit-error  rates;  resolution;  sample 
rates;  anti-jamming  margins;  cost 

From  the  mission  analysis,  it  is  clear  that  the  MOEs  for  the  DSN  are  the  accurate 
identification  of  a  SAW  with  the  intention  of  crashing  into  Jurong  Island  and  the  earliest 
impact  warning  the  DSN  can  provide.  In  the  next  section,  the  MOP  will  be  developed 
after  a  requirement  analysis  is  conducted  to  support  this  MOE. 
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E.  REQUIREMENTS  ANALYSIS 

The  requirements  analysis  is  built  on  the  needs  analysis  and  comprises  an 
operational-requirements  analysis,  a  functional  analysis,  and  a  non-functional  analysis. 
The  goal  of  the  requirements  analysis  is  to  generate  MOP(s). 

1.  Operational  Requirements  Analysis 

Operational  requirements  are  derived  directly  from  the  problem  statement.  The 
top-level  need  is  to  provide  accurate  early  warning  of  a  SAW  traveling  east  to  west  on  the 
TSS,  abruptly  turning,  and  accelerating  towards  Area  A1  of  Jurong  Island  at  full  speed, 
which  is  deemed  the  worst-case  scenario.  Hence,  the  operational  requirement  for  the 
sensor  network  mandates  that  a  SAW  be  identified  early  and  with  high  confidence  before 
it  reaches  Area  Al. 

2.  Functional  Analysis 

Functional  analysis  involves  identifying  the  top-level  functions  of  the  sensor 
network  and  performing  functional  decomposition,  as  captured  in  a  functional- 
requirements  diagram  [21]. 

a.  Functional  Requirements 

The  use  case  diagrams  in  this  section  help  illustrate  what  the  sensor 
network  is  doing  at  any  given  time.  A  complete  set  of  use  case  diagrams  captures  all  the 
functions  the  sensor  network  should  perform  [37]. 
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Figure  12  displays  the  use  case  diagram  for  the  sensor-network  problem 
description.  The  sensor  platforms  (i.e.,  MPA  radar  tower,  PV,  CPC,  and  MPA)  are 
depicted  as  actors  perfonning  their  own  SAW  detection  and  tracking.  The  respective 
HQs  (i.e.,  MPA,  RSN,  PCG,  and  RSAF)  of  the  sensor  platforms  may  or  may  not  carry 
out  the  tracking  of  the  SAW.  Depending  where  data  fusion  is  conducted,  the  fusing  of 
data  and  classifying  of  vessels  may  be  perfonned  either  at  the  sensor  platfonns,  the 
respective  HQs  of  the  sensor  platforms,  or  the  SMSC.  Like  the  HQs,  SMSC  may  or  may 
not  track  the  SAW. 


uc  SingaporeMDASensorNetwork  [TopLevelUseCase] 


Figure  12.  Singapore  MDA  Sensor  Network,  Use  Case  Diagram 


b.  Functional  Decomposition 

The  requirements  diagram  (Figure  13)  captures  the  functional 
decomposition  of  the  sensor  network.  Each  Tier  1  function  is  further  decomposed  into 
sub-functions  to  provide  better  resolution  concerning  what  is  required  of  the  sensor 
network.  For  example,  C3  is  deliberately  treated  as  a  separate  function,  but  asserts 
command  and  control  via  the  communications  (Comm)  sub-function. 
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req  SingaporeMDASensorNetworkRequirements [Functional] 


req  SingaporeMDASensorNetworkRequirements [Functional] 


Figure  13.  Singapore  MDA  Sensor  Network  Requirements  Diagram 
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3. 


Measures  of  Performance 


The  mission  of  the  sensor  network  is  to  provide  accurate  identification  of  SAW 
intent  and  impact  warning  well  before  it  reaches  Jurong  Island.  In  order  to  achieve  these 
MOEs,  one  possible  measure  of  performance  (MOP)  would  be  how  fast  the  sensor  fusion 
algorithms  can  be  executed.  This  MOP  will  not  be  explored  further  for  the  purpose  of 
this  thesis,  but  it  is  assumed  to  be  identical  regardless  of  the  DSN  architecture  chosen  and 
sensor-fusion  algorithm  used. 

F.  SUMMARY 

This  chapter  discusses  the  relationships  between  maritime  domain  security, 
maritime  domain  awareness,  network-centric  operations,  and  data  fusion.  It  is  followed 
by  an  elucidation  of  the  problem  statement  for  this  research  and  the  mission  analysis. 
The  mission  analysis  details  the  threats  and  their  possible  attack  approaches,  rationalizes 
the  potential  targets,  and  describes  the  available  sensors,  sensor  platforms,  and  fusion 
centers,  and  the  physical  environment  in  which  the  mission  is  to  be  executed.  As  a  result 
of  the  needs  and  requirements  analyses,  the  MOE  and  MOP  are  developed. 
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III.  DISTRIBUTED  SENSOR  NETWORK  ARCHITECTURES 


A.  INTRODUCTION 

The  problem  statement,  mission  analysis,  needs  analysis,  and  requirements 
analysis  now  lead  to  the  generation  of  alternative  sensor  network  architectures.  The 
existing  sensors  and  sensor  platforms  described  in  Chapter  II  form  the  various  DSN 
architectures.  The  key  differences  between  the  architectural  alternatives  depend  on 
“where”  the  fusion  centers  are  placed  in  the  network  and  the  data  type  (i.e.,  raw  data  or 
processed  tracks)  to  be  fused  at  the  fusion  centers.  In  the  following  sections,  three  broad 
DSN  architectures  are  presented,  followed  by  a  discussion  of  how  the  current  suite  of 
sensors,  sensor  platforms,  and  fusion  centers  are  embedded  into  each  of  the  three 
architectures. 

B.  FUSION  ARCHITECTURES 

Data  fusion  involves  the  integration  of  sensors,  data  processing,  and  estimation. 
It  demands  organization,  whether  structured  or  unstructured,  between  the  sensors  and  the 
sensor  platforms  that  acquire  and  process  the  data.  This  organization  is  dictated  by  the 
sensor  network  architecture,  and  there  are  three  main  types. 

1.  Centralized  Data  Fusion  Architecture 

For  centralized  data  fusion  architecture  [38],  there  is  only  one  fusion  center, 
serving  as  a  central  processor  that  collects  all  raw  data  from  the  different  sensors  (Figure 
14a).  Though  this  research  does  not  discuss  the  dissemination  of  infonnation  from  the 
fusion  center,  it  should  be  understood  that  data  or  infonnation  flows  do  not  necessary  end 
there. 

The  main  advantage  of  centralized  fusion  is  it  offers  the  best  theoretical 
performance  in  terms  of  minimizing  enors  and  data  loss.  The  disadvantages,  however, 
are  many.  Communications  bottleneck  is  possible  because  all  sensors  vie  to 
communicate  directly  with  the  fusion  center,  which  ironically  leads  to  data  loss  and 
reduced  perfonnance.  Vulnerability  is  a  concern  because  failure  in  the  central  processor 
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or  of  communications  links  could  cripple  the  entire  sensor  network.  Finally,  centralized 
architecture  affords  poor  modularity  and  scalability,  as  any  addition  of  sensors  will 
usually  require  more  computing  power  from  the  fusion  center  and  a  higher  network- 
communications  bandwidth. 

2.  Decentralized  Data  Fusion  Architecture 

Decentralized  data  fusion  architecture  [38-39]  (Figure  14b)  features  an 
organization  of  sensors  directly  opposite  to  that  of  centralized  data  fusion  architecture. 
For  this  architecture,  each  sensor  node  (i.e.,  sensor  platform)  takes  on  the  role  of  fusing 
data  captured  by  its  own  sensors  and  data  from  the  rest  of  the  sensor  nodes  connected  to 
it,  rather  than  relying  on  a  single,  central  fusion  center.  The  advantages  of  the 
decentralized  data  fusion  architecture  are  the  increased  survivability  of  the  sensor 
network  (by  eliminating  the  single  point  failure  of  the  single  central  processor),  improved 
modularity  and  scalability  (by  making  local  changes  to  the  network),  and  the  absence  of 
constraints  imposed  by  a  central  computing  power. 

The  decentralized  data  fusion  architecture  is,  however,  not  without  shortfalls. 
One  serious  problem  is  redundant  data  transmission.  When  this  occurs,  data  may  be 
reused  (i.e.,  double  counted)  and  result  in  fused  estimates  being  overly  optimistic. 

3.  Hybrid  Data  Fusion  Architecture 

A  hybrid  sensor  network  architecture  [38,  40]  (Figure  14b),  also  referred  to  as 
hierarchical  architecture,  involves  both  centralized  and  decentralized  data  fusion  schemes. 
In  general,  the  motivation  for  using  decentralized  data  fusion  is  to  reduce  computational 
workload  and  communication  demands,  while  the  incentive  for  using  centralized  data 
fusion  is  better  accuracy.  The  hybrid  data  fusion  offers  a  middle  ground,  combining  the 
advantages  of  the  centralized  and  decentralized  architectures  without  some  of  their 
disadvantages. 

In  a  hybrid  architecture,  there  are  often  several  hierarchical  levels,  where  the  top 
level  contains  a  single,  centralized  fusion  center  and  the  lower  levels  contain  several 
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decentralized  (i.e.,  local)  fusion  nodes.  Depending  on  the  organization  of  the  sensors  and 
sensor  platforms,  each  sensor  platfonn  may  receive  data  from  a  cluster  of  sensors  or  from 
other  sensor  platforms. 


(b) 


Figure  14.  Three  Sensor  Data-fusion  Architectures:  (a)  Centralized  Processing  at  the 
Fusion  Center,  (b)  Decentralized  Processing  at  Each  Node-processing  Unit, 
and  (c)  Hierarchical  Processing.  (After:  [40]) 
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c. 


ARCHITECTURE  ALTERNATIVES 


In  this  section,  different  architectures  are  developed,  taking  into  consideration 
existing  sensors,  sensor  platforms,  and  fusion  centers  used  by  Singapore,  as  described  in 
the  previous  chapter.  Figure  15  illustrates  a  simplified  command  and  control  (C2) 
structure  within  and  between  the  various  maritime-security  agencies.  The  vertical  divider 
depicts  the  intra-agency  C2  structure,  and  the  horizontal  divider  shows  the  chain-of- 
command  equivalency  between  the  agencies. 


Figure  15.  Command  and  Control  (C2)  Structure  within  and  between  the  Various 

Maritime  Security  Agencies 
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1. 


Centralized  Architecture 


In  a  centralized  DSN  architecture  based  on  C2  structure  (Figure  15),  the  SMSC  is 
assigned  as  the  central  fusion  center.  Raw  data  captured  by  the  bottom  tier  is  sent  to  the 
SMSC  via  the  intermediate  tier.  For  this  architecture,  the  intermediate  tier  does  not  carry 
out  fusion  of  data,  but  simply  forwards  it  to  the  SMSC  for  fusion.  On  completion  of  data 
fusion  by  the  SMSC,  the  fused  track  (i.e.,  processed  data)  is  disseminated  to  the 
intennediate  and  bottom  tiers  (Figure  16). 


TOP  TIER 


Figure  16.  Data  Flow  for  Centralized  Architecture 
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2. 


Decentralized  Architecture 


For  decentralized  DSN  architecture,  each  individual  sensor  platform  in  the  bottom 
tier  is  a  fusion  node.  As  illustrated  in  Figure  17,  each  sensor  platform  fuses  raw  data 
captured  by  its  own  sensors  and  by  other  sensor  platforms.  The  fusion  algorithm  used  by 
the  sensor  platforms  is  assumed  to  be  identical.  Consequently,  given  the  same  raw  data 
input  at  each  sensor  platfonn,  the  fused  track  will  be  identical.  Hence,  dissemination  of 
the  fused  track  is  simplified,  as  there  is  no  requirement  for  cross  dissemination  (e.g.,  the 
fused  tracks  from  a  PV  do  not  need  to  be  sent  to  PCG  HQ).  The  sensor  platform  is  only 
required  to  disseminate  the  fused  track  to  its  own  chain  of  command  for  its  awareness. 
The  SMSC  receives  multiple  identical  tracks,  which  is  a  redundancy  feature  expected  of 
the  decentralised  architecture.  No  processing  is  required  by  the  intennediate  and  top  tiers 
in  this  architecture. 


TOP  TIER 


Figure  17.  Data  Flow  for  Decentralized  Architecture 
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3.  Hybrid  Architecture 

The  hybrid  DSN  architecture,  unlike  the  previous  two,  has  both  data  fusion  (i.e., 
fusion  of  raw  data)  and  track-to-track  fusion  (i.e.,  fusion  of  tracks),  carried  out  at 
different  tiers.  Each  individual  sensor  platform  in  the  bottom  tier  first  fuses  the  raw  data 
captured  by  its  own  sensors,  then  disseminates  the  fused  track  for  fusion  at  either  the 
intermediate  or  top  tier.  In  this  work,  the  SMSC  is  selected  to  carry  out  track-to-track 
fusion.  The  choice  between  the  intennediate  tier  and  the  top  tier  for  track  fusion,  as  will 
be  seen  later,  results  in  very  little,  if  any,  difference  in  this  simulative  study.  However, 
since  the  SMSC  is  used  as  the  fusion  center  for  the  centralized  architecture,  for 
consistency  the  SMSC  is  also  assumed  to  carry  out  the  “final”  fusion  for  the  hybrid 
architecture.  As  illustrated  in  Figure  18,  every  sensor  platform  fuses  raw  data  captured 
by  its  own  sensors.  The  fused  track  is  sent  to  the  SMSC  via  its  own  chain  of  command. 
The  SMSC  pools  the  individually  fused  tracks  and  carries  out  track-to-track  fusion.  The 
final  fused  track  is  then  disseminated  to  the  chains  of  command  for  their  awareness.  No 
processing  is  required  by  the  intermediate  tier  in  this  case. 


Figure  18.  Data  Flow  for  Hybrid  Architecture 
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D.  SUMMARY 

In  this  chapter,  three  types  of  DSN  architectures — centralized,  decentralized,  and 
hybrid — are  described.  These  architectures  are  formed  using  the  existing  sensors,  sensor 
platforms,  and  fusion  center  used  by  Singapore.  The  choice  of  data-fusion  algorithm  to 
be  employed  in  each  architectural  alternative  is  discussed  in  the  next  chapter.  Together, 
they  are  evaluated  using  modeling  and  simulation,  discussed  in  Chapter  V,  to  determine 
which  sensor-network  architecture  affords  the  earliest  warning  and  best  identification 
accuracy  of  a  SAW  intending  to  attack  Jurong  Island. 
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IV.  SENSOR  FUSION  ALGORITHMS 


A.  INTRODUCTION 

In  their  revision  of  the  Joint  Directors  of  Laboratories  (JDL)  Data  Fusion  Group’s 
data  fusion  model,  Steinberg  et  al.  offer  a  succinct  definition  of  data  fusion  as  “the 
process  of  combining  data  or  infonnation  to  estimate  or  predict  entity  states”  [41].  In  the 
following  sections,  the  focus  is  on  the  sensor  fusion  algorithms  used  in  this  research  to 
combine  sensor  data  to  estimate  the  SAW  state.  A  detailed  derivation  of  the  algorithms  is 
not  provided  here,  as  these  algorithms  can  be  found  in  most  sensor-fusion  (e.g.,  [38],  [40], 
[43]  and  [46]). 

B.  DATA  FUSION  MODELS 

Many  existing  data  fusion  models  [40]  help  generalize  and  guide  the  data  fusion 
process.  Referencing  the  JDL  fusion  model  (Figure  19),  the  process  of  data  fusion  is 
divided  into  levels  or  hierarchies.  At  the  lower  levels  (levels  0-2),  the  processes  are 
concerned  with  track  formation,  identity,  or  estimated  infonnation  and  the  fusion  of 
information  from  several  sources.  At  the  higher  levels  (levels  3-4),  the  process  is 
concerned  with  the  extraction  of  “knowledge”  or  decisional  information. 

In  this  work,  Level  1  deals  with  fusion  of  ship  kinematics,  which  involves  fusion 
of  local  infonnation  to  detennine  the  position,  velocity,  and  acceleration  of  a  detected 
ship.  Level  2  seeks  to  discover  the  relationship  between  the  ship  and  its  navigational 
pattern.  Level  3,  based  on  predictions  about  the  future  situation,  is  an  estimation  of 
danger  (i.e.,  threat  assessment),  as  to  whether  a  vessel  is  aiming  for  Jurong  Island  (i.e.,  a 
SAW).  Level  4  forms  the  process  of  dynamic  resource  reallocation.  For  evaluation 
purposes,  Level  4  is  predetennined  under  sensor-coverage  analysis  in  Chapter  II  and  used 
as  fixed  input  for  the  simulative  study  discussed  in  the  next  chapter.  In  other  words,  the 
commitment  and  allocation  of  the  number  of  sensors  is  classified  into  best  and  worst-case 
scenarios  up  front,  and  they  fonn  the  basis  for  evaluation  for  each  DSN  architecture. 
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Figure  19.  Joint  Directors  of  Laboratories  Data  Fusion  Model  (From:  [42]) 


C.  STATE  MODEL 


In  this  research,  ship  motion  is  simulated  using  a  constant-velocity  model  (CVM). 
The  ship’s  acceleration  can  be  accounted  for  in  the  process  noise.  Therefore,  the  explicit 
use  of  a  constant  acceleration  model  is  not  necessary.  In  this  model,  the  state  (or  state 
vector)  at  time  tk+i  is  obtained  estimated  according  to 

x(k+l)  =  F(k)x(k),  (1) 

where 


x(k+l)  = 


F(k)  = 
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AT  is  the  time  step  between  time  tk  and  time  tk+i,  for  k  =  0,  1,  2,  3, 
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D.  MEASUREMENT  MODEL 


The  measurement  (observation)  at  time  tk,  z(k),  is  modeled  according  to 
z(k)  =  H(k)x(k)  +  v(k), 

where 


z(k) 


H(k)  = 


f  1 

v° 


with  components  x  and  y, 


0  0 
0  1 


(F 

0, 


is  the  measurement  matrix,  and 


(2) 


v(k)  ~  N(0,  R(k))  ,  the  measurement  noise,  is  assumed  to  be  Gaussian,  temporally 
uncorrelated,  and  of  zero-mean  and  covariance  R(k). 

Note  that  observations  (range  and  bearing)  made  by  radar  and  electro-optical 
devices  expressed  in  a  polar  coordinate  system  are  converted  to  those  expressed  in  a 
Cartesian  coordinate  system  before  filtering  or  fusion  takes  place. 


E.  ESTIMATION  FOR  DATA  FUSION 

Estimation  is  the  “single  most  important  problem”  [43]  in  data  fusion.  Sensors 
make  observations  (i.e.,  take  measurements)  for  the  purpose  of  producing  an  estimate  of 
the  true  state  of  the  environment  being  observed.  The  better  the  estimation,  the  closer  the 
estimate  will  be  to  the  truth.  In  addition,  an  estimate  is  considered  good  if  its  uncertainty 
is  small. 

Given  time  constraints,  it  was  not  possible  to  investigate  all  the  available  data 
fusion  algorithms  for  this  research.  Taking  into  consideration  that  the  chosen  state  and 
sensor  models  are  linear  and  well  defined,  and  that  both  the  process  and  observation 
noises  are  assumed  to  be  Gaussian,  temporally  uncorrelated,  and  of  zero-mean,  the 
Kalman  filter  (KF)  and  infonnation  filter  (IF)  are  chosen  from  among  other  alternatives 
(Bayesian  filter,  particle  filter,  extended  Kalman  filter,  etc.).  Following  are  the  KF  and 
IF  algorithms  for  state  and  uncertainty  estimation,  using  a  set  of  observations  captured  by 
the  sensors. 
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1.  Kalman  Filter  Algorithm 

The  Kalman  filter  (KF)  algorithm,  extracted  from  [43],  is  now  stated  without 
proof.  Derivations  of  the  KF  equations  can  also  be  found  in  many  books  on  KF[40,44- 
45].  The  state  is  assumed  to  evolve  in  time  according  to  Equation  1.  Observations  of  this 
state  are  modeled  according  to  Equation  2.  The  assumptions  previously  made  about  the 
noises  are  applied. 

The  predicted  state  x  (k|k-l)  and  covariance  matrix  P(k|k-1)  at  time  tk  are 
obtained  according  to 

x  (k|k- 1 )  =  F(k)  x  (k- 1  |k- 1 ),  (3) 

P(k|k-1)  =  F  (k)P(k- 1  |k- 1  )FT(k)  +  Q(k),  (4) 

where 

T  denotes  the  transpose  of  a  matrix, 

V  0  0  0 

0g  x20  0 

Q(k)  ~  o  a  o  o  y2 

0  g  0  0  0y2 

process  noise  matrix.  The  process  noise  is  assumed  to  be  Gaussian,  temporally 
uncorrelated,  and  of  zero-mean  and  variances,  ox2  and  oy2  of  the  x-  and  y-component  of 

2  2 

the  ship’s  position  vector,  respectively,  and  a.  “and  of  are  the  variances  of  the  x-  and  y- 
component  of  the  ship’s  velocity  vector,  respectively. 

The  measurement  z(k)  that  is  available  at  time  tk  is  used  to  update  the  state  and 
the  covariance  matrix.  The  updated  state  x  (k|k)  and  covariance  matrix  P(k|k)  at  time  tk 
are  computed  according  to 

x  (k|k)  =  x  (k|k-l)  +  W(k)[z(k)  -  H(k)  x  (k|k-l)],  (5) 

P(k|k)  =  P(k|k- 1)  -  W(k)S(k)WT(k),  (6) 

where  the  gain  matrix  W  and  innovation  variance  matrix  S  are  given  by: 
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S(k)  =  H(k)P(k|k- 1  )HT(k)  +  R(k), 

W(k)  =  P(k|k- 1  )HT(k)S"'(k), 

and  R(k)  is  the  measurement  noise  covariance  matrix. 

The  KF  is  recursive  (Figure  20).  The  process  starts  with  an  initial  estimate  of  the 
state  and  the  covariance  matrix,  propagates  them,  and  updates  them  when  an  observation 
becomes  available. 


Figure  20.  Block  diagram  of  the  Kalman  filter  cycle  (After:  [44]) 
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2.  Information  Filter  Algorithm 

Whereas  the  KF  deals  with  the  estimation  of  the  state  vector  x  and  its  covariance 
matrix  P,  the  information  filter  (IF)  algorithm,  extracted  from  [43],  deals  with  the 
infonnation  state  vector  y  =  P’1  x  and  the  infonnation  matrix  Y=  P  . 

The  predicted  information  state  vector  y(k|k-l)  and  the  predicted  infonnation 
matrix  Y(k|k-1)  at  time  tk  are  obtained  according  to 

y  (k|k- 1 )  =  [1  -  ft(k)GT(k)]FT(k)y  (k-l|k-l),  (7) 

Y(k|k-1)  =  M(k)  -  H(k) 
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3.  Comparing  KF  and  IF 

A  comparison  of  the  updated  algorithms  between  KF  (equations  5  and  6)  and  IF 
(Equations  9  and  10)  highlights  the  simplicity  of  the  IF  update  over  the  KF  update.  In 
information  fonn,  the  update  stage  is  a  straight  addition  of  infonnation  from  an 
observation  to  a  prediction.  This  simplicity  gives  the  IF  its  advantage  over  KF  in 
multisensor  estimation  problems.  Any  change  to  the  number  of  sensors  is  merely  an 
addition  or  subtraction  of  information.  However,  the  simplicity  of  the  update  stage  of  the 
information  filter  comes  at  the  cost  of  increased  complexity  in  the  prediction  stage. 

4.  Track-to-Track  Fusion 

The  track-to-track  fusion  algorithm  combines  track  estimates  from  sensors, 
making  it  distinctly  different  from  the  KF  and  IF  algorithms,  which  combine  observations 
(to  form  tracks)  from  different  sensors.  In  this  thesis,  track-to-track  fusion  is  referred  to 
as  track  fusion,  while  fusion  of  observations  is  called  data  fusion. 

To  carry  out  track  fusion,  data  fusion  using  either  the  KF  or  IF  must  be  executed 
first.  Each  sensor  generates  its  own  track  locally  using  the  KF  or  IF,  and  the  track  is  then 
sent  to  a  fusion  center,  where  it  is  combined  with  other  tracks  to  generate  a  global  track 
estimate.  The  main  advantage  of  track  fusion  over  data  fusion  is  that  sending  track 
information  takes  up  less  communications  bandwidth,  due  to  its  smaller  data  size. 
However,  due  to  the  fusion  of  filtered  estimates,  the  global  track  may  result  in  a  poorer 
global  estimate  with  higher  uncertainty.  This  will  be  discussed  in  detail  in  the  next 
chapter. 

The  track-to-track  fusion  (also  commonly  referred  to  as  track  fusion)  algorithm 
extracted  from  [46]  is  stated  here  without  derivation.  Prior  to  fusing  the  individual  track 
estimates,  a  track  association  test  is  carried  out  to  ensure  that  the  tracks  are  likely  to 
belong  to  the  same  contact  of  interest.  Only  when  the  tracks  pass  the  association  test  will 
they  undergo  the  track  fusion  process. 
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a. 


Track  Association  Test 


At  each  time  step,  the  algorithm  uses  the  estimated  state  of  the  target  from 
sensor  i,  x'(k),  and  that  from  sensor  j,  x'  (k),  and  their  corresponding  covariances  P'(k) 
and  P'(k),  respectively.  The  state  estimation  errors  are  assumed  to  be  independent.  For 
simplicity,  the  time  argument  will  be  omitted  from  here  on. 

The  track  association  test  is  passed  if 
D  ^  ( Aij  )T(Tij)_1(  Aij )  <  Da, 

where 

2 

Da  —  5Cn  (1-  a),  the  threshold,  is  detennined  using  a  chi-square  distribution 
with  n  degrees  of  freedom  and  confidence  level  of  a, 

Alj  =  x1  -  \j , 

T1J  =  P1  -  P’,  the  covariance  of  A'J . 

b.  Fusion  of  Estimates 

Fusion  of  two  estimates  x'  and  ±J  and  their  respective  covariances  P1  and 
PJ  results  in  the  estimated  track  and  its  covariance: 


X  =Pj(Pi  +  Pi)-1x‘  +Pi(Pi  +  Pi)‘1xS 

(11) 

p  =  P(P'  +  Pi)-1pi. 

(12) 

F.  SUMMARY 

This  chapter  begins  with  a  recap  on  data  fusion  and  provides  a  brief  discussion  of 
the  data  fusion  model  developed  by  JDL.  Next,  the  state  and  sensor  models,  together 
with  the  KF,  IF  and  track-to-track  fusion  algorithms,  are  stated  without  derivation.  In  the 
next  chapter,  the  materials  covered  in  Chapters  III  and  IV  are  modeled  and  simulated 
using  MATLAB  to  find  which  sensor  network  architecture,  complemented  with  a  suitable 
fusion  algorithm,  produces  the  earliest  and  most  accurate  identification  of  a  SAW 
intending  to  strike  Jurong  Island. 
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V.  SIMULATIVE  STUDY 


A.  INTRODUCTION 

In  this  chapter,  the  scenario  of  a  SAW  navigating  the  Singapore  Straits  with  the 
intention  of  crashing  into  the  oil  and  chemical  tenninals  on  Jurong  Island  is  modeled, 
followed  by  the  simulation  of  alternative  sensor  network  architectures  and  different 
sensor  fusion  algorithms.  The  outcomes  of  the  modeling  and  simulation  are  the  estimated 
MOPs  and  MOEs  for  each  alternative  that  will  be  used  to  detennine  the  best  sensor- 
network  architecture  and  sensor-fusion  algorithm  pair  to  deploy  against  the  SAW.  This 
evaluation  is  a  culmination  of  the  SoSADP — the  system  of  systems  architecture  ranking. 

B.  MODELING  AND  SIMULATION 

In  the  real  world,  many  MDS  and  MDA  problems,  such  as  those  treated  in  this 
research,  are  impossible  to  observe  and  study.  Even  if  it  were  possible,  it  would  take 
many  resources  to  set  up  and  conduct  actual  experimental  trials,  validation  trials,  and  also 
re-validation  trials.  Hence,  the  use  of  modeling  and  simulation  (M&S)  in  this  study  is  the 
prudent,  if  not  the  only,  option  for  understanding  and  experimenting  with  the  MDA 
problem. 

1.  Model  Development 

MATLAB  (Ver.  R2010a)  software  serves  as  the  simulation  environment  for 
coding  and  simulation.  The  characteristics  and  attack  approach  of  the  SAW,  its  target  on 
Jurong  Island,  the  sensors  and  their  network  topology,  and  the  sensor  platforms  and 
fusion  centers  and  their  locations,  are  created  according  to  the  problem  statement, 
mission  analysis,  needs  analysis,  and  requirements  analysis  in  Chapter  II. 

a.  The  Ship  as  a  Weapon 

In  this  simulative  study,  the  SAW  is  the  only  target  (that  is,  this  is  a 
single-target  simulation).  It  starts  its  passage  from  the  east  of  the  Singapore  Strait  and 
cruises  in  the  TSS  at  15  knots.  At  the  closest  point  of  approach  from  the  TSS  to  Area  A1 
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of  Jurong  Island,  the  SAW  turns  and  heads  towards  Area  A1  and  accelerates  to  20  knots. 
The  navigation  route  and  attack  approach  taken  by  the  SAW  is  illustrated  in  Figure  5  in 
Chapter  II. 


b.  Sensor  Topology 

The  MPA  radar  stations  are  modeled  as  fixed  sensors  on  stationary 
platforms  with  a  line-of-sight  range  of  15  mn.  Up  to  five  such  fixed  sensors  can  be  used 
in  the  simulation,  and  their  predetennined  locations  are  evenly  distributed  along  the 
southern  coast  of  Singapore  (Figure  10  in  Chapter  II).  Augmenting  these  fixed  sensors 
are  four  mobile  sensors:  two  CPCs,  one  PV,  and  one  MPaA.  Though  they  are  mobile, 
each  having  its  own  patrol  route,  in  this  simulation,  their  sensor  ranges  are  extended  such 
that  they  have  complete  coverage  of  the  Singapore  Strait.  This  is  done  to  simplify  sensor 
management  where  there  will  be  more  sensors  deployed  to  keep  an  eye  on  suspicious 
vessel  behavior.  This  assumption  does  not  infringe  on  realism  because  the  interesting 
comparison  here  lies  in  the  number  of  sensors  monitoring  the  SAW  (i.e.,  analyzing 
whether  more  sensors  would  result  in  earlier  warning  and  more  accurate  identification  of 
the  SAW).  In  addition,  sensor  errors  are  assumed  to  be  identical  for  all  sensors,  because 
the  purpose  here  is  to  detennine  the  best  network  architecture  and  not  which  types  of 
sensor  to  use  in  the  network. 

c.  Target  Destination 

As  identified  and  explained  in  Chapter  IV,  Area  A1  of  Jurong  Island  is  the 
most  attractive  and  easiest  to  strike  with  a  SAW.  Its  location  is  presented  as  a  circle, 
using  an  estimated  latitude  and  longitude  as  center  and  a  radius  of  0.5  mn.  This  circle 
forms  the  basis  for  threat-level  assessment,  which  is  discussed  later  in  this  chapter. 

d.  Sensor  Network  Architecture  and  Fusion  Algorithm 

In  the  simulative  study,  three  sensor-network  architectures — centralized, 
decentralized,  and  hybrid — are  modeled  in  accordance  with  Figures  16,  17,  and  18  in 
Chapter  IV.  The  sensor  algorithms — Kalman  filter,  infonnation  filter,  and  track-to-track 
fusion — are  applied  to  the  architectures  as  listed  in  Table  4. 
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Table  4.  Applying  Algorithms  to  Architectures 


Architecture 

Algorithm(s) 

Applied 

Purpose  /  Remarks 

Centralized 

Sensor 

Network 

Kalman 

Filter  and 

Infonnation 

Filter 

With  the  assumption  that  the  measurements  made  are  of 
Gaussian  noise  with  zero  mean  and  constant  covariance, 
the  KF  and  IF  will  produce  the  same  data  fusion  results. 
Hence,  this  case  seeks  to  validate  this  (which  at  the 
same  time  proves  that  the  algorithms  are  implemented 
conectly). 

Decentralized 

Sensor 

Network 

Infonnation 

Filter 

This  case  seeks  to  validate  that  the  decentralized  and 
centralized  sensor  network  architectures  produce  the 
same  results.  Only  the  IF  is  applied  since  both  KF  and 
IF  produce  the  same  results  regardless  of  sensor  network 
in  the  context  of  this  thesis.  The  reason  for  choosing  IF 
over  KF  will  be  discussed  later  in  this  chapter. 

Hybrid  Sensor 
Network 

Infonnation 
Filter  and 
Track-to- 
track  fusion 
algorithm 

IF  is  used  to  carry  out  data  fusion,  followed  by  the 
implementation  of  a  track-to-track  fusion  algorithm  to 
carry  out  track  fusion.  The  results  are  compared  to  the 
other  two  architectures  using  IF. 

2.  Simulation  Program  Description 

The  simulation  program  builds  upon  the  initial  MATLAB  programs  produced  by 
Durrant-Whyte  [43].  Many  modifications  and  additions  are  made  to  adapt  the  code,  but 
the  functional  flow  of  the  program  remains  largely  the  same.  The  simulation  program  is 
made  up  of  four  major  blocks:  (1)  centralized  sensor-network  architecture  with  KF,  (2) 
centralized  sensor  network  architecture  with  IF,  (3)  decentralized  sensor  network 
architecture  with  IF,  and  (4)  hybrid  sensor  network  architecture  with  IF  and  a  track  fusion 
algorithm.  There  are  multiple  functions  and  sub-functions  contained  within  each  block. 
Each  major  block  is  designed  to  be  able  to  execute  as  a  standalone  (i.e.,  to  generate  its 
own  sensor  measurements  and  then  carry  out  fusion)  or  in  combination  with  each  other 
(i.e.,  use  previously  generated  sensor  measurements  by  another  block  to  carry  out  fusion). 
This  modularity  in  design  enables  data  portability  between  blocks  and  facilitates  future 
research  work.  Each  block  and  some  of  the  significant  functions  are  described  below. 
The  full  MATLAB  code  is  provided  in  Appendix  B. 
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a. 


Centralized  Sensor  Network  Architecture  With  Kalman  Filter 


This  block  sets  up  the  sensor  network  as  a  centralized  architecture.  The 
measurements  made  by  the  sensors  of  the  SAW  are  sent  to  a  single  fusion  center  for  data 
processing.  The  fusion  center  implements  the  KF  to  fuse  all  the  sensor  data.  Some  of  the 
more  important  functions  are  described  in  the  following. 

•  example_sim.m:  This  script  file  sets  up  the  mission  environment 
(i.e.,  the  location  of  Area  A1  of  Jurong  Island,  and  the  TSS  and 
port  limits  of  Singapore  Strait),  sensor  topology  (i.e.,  the 
disposition  of  the  fixed  and  mobile  sensors),  the  SAW  trajectory 
(i.e.,  navigational  passage  in  the  Singapore  Strait  and  attack 
approach),  and  generates  the  sensors  measurements  made  of  the 
SAW.  Some  of  the  important  functions  associated  with  this  script 
are: 

o  ginit.m:  Sets  the  values  used  for  the  simulation  parameters 
o  generate_platforms.m:  Creates  the  different  types  and  number 
of  platforms  based  on  user  requirements.  The  disposition  and 
trajectory  of  the  platforms  are  described  in 

platfor  mstepxxx.  m . 

o  generate_targets.m:  Creates  the  SAW  and  its  initial  location. 

The  SAW  trajectory  is  described  in  target_step_SAW_path.m. 
o  assignsensors.m:  The  user  can  specify  the  specifications  of 
the  sensors  such  as  the  platform  they  belong  to,  maximum 
range,  beam  width,  measurement  errors,  etc., 
o  generateobservations.m  and  sensorreport.m:  Generates 
the  sensor  measurements  made  of  the  SAW.  The  true  target 
location  and  true  platform  location  are  detennined  at  each  time 
step  and  the  true  range  and  bearing  of  the  target  from  the 
sensor  is  computed.  If  this  range  and  bearing  are  within  the 
sensor  range,  the  measurement  is  valid  and  noise  is  added. 

•  run_filt.m:  This  script  file  runs  the  KF  for  a  multi-sensor,  single¬ 
target  scenario.  It  begins  by  setting  up  filter  parameters  then 
applies  the  KF  on  the  sensor  measurements  made.  Some  of  the 
important  functions  associated  with  this  script  are: 

o  init_track.m:  Extracts  the  first  few  observations  and  uses  them 
to  initialize  the  fused  track 

o  xy_obs.m:  Converts  the  sensor  measurement  in  range-bearing 
form  to  x-y  form 

o  make_track.m:  Initializes  the  track-data  structure  and  contains 
predicted  and  estimated  states,  respective  covariances,  and 
corresponding  innovations 

o  state_pred.m:  Executes  the  state  and  covariance  prediction 
using  equations  3  and  4  shown  in  Chapter  IV 
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o  data_association.m:  Associates  sensor  reports  to  the  target 
o  state  update.m:  Executes  the  state  and  covariance  update 
using  equations  5  and  6  shown  in  Chapter  IV. 

b.  Centralized  Sensor  Network  Architecture  With  Information 
Filter 

The  generation  of  sensor  measurements  for  this  block  is  the  same  as  that 
elaborated  for  the  centralized  sensor  network  architecture  with  KF.  The  functions  such  as 
example_sim.m,  ginit.m,  and  so  on,  are  identical.  However,  instead  of  the  KF,  the  IF  is 
implemented  here.  The  IF  is  responsible  for  fusing  all  the  sensor  data  sent  by  the 
individual  sensors  to  the  fusion  center.  Some  of  the  more  important  functions  which  are 
different  from  the  above  are  now  described. 

•  runifilt.m:  This  script  file  runs  the  IF  for  a  multi-sensor,  single¬ 
target  scenario.  This  can  be  run  on  the  same  data  generated  for  the 
KF  so  that  the  two  methods  may  be  compared.  Fike  runfilt.m, 
this  file  begins  by  setting  up  filter  parameters  and  then  applies  the 
IF  on  the  sensor  measurements  made.  Its  corresponding  functions 
are: 

o  init_itrack.m:  Initializes  the  fused  track, 
o  make_itrack.m:  Initializes  the  track  data  structure  and 
contains  predicted  and  estimated  states,  and  their  respective 
covariances 

o  info_pred.m:  Executes  the  state  and  covariance  prediction 
using  equations  7  and  8  shown  in  Chapter  IV 
o  info_update.m:  Executes  the  state  and  covariance  update 
using  equations  9  and  10  shown  in  Chapter  IV. 

c.  Decentralized  Sensor  Network  Architecture  with  Information 
Filter 

This  decentralized  sensor  network  code  simulates  a  SAW  (i.e.,  single 
target)  being  tracked  by  multiple  sensors.  Each  sensor  acts  as  a  fusion  center  capable  of 
receiving  measurement  data  from  all  the  other  sensors  in  the  network  and,  together  with 
its  own  measurement  data,  constructing  a  fused  track.  As  with  the  centralized  sensor 
network,  the  user  is  allowed  to  specify  up  to  nine  sensors — five  fixed  and  four  mobile — 
to  make  up  the  network  of  sensors.  This  block  is  compatible  with  the  previous  two 
blocks  in  such  a  way  that  sensor  measurements  generated  by  example_sim.m  can  be 
used  here.  Some  of  the  important  functions  follow. 
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•  run_net.m:  This  script  file  is  similar  to  example_sim.m.  It  sets 
up  the  mission  environment,  sensor  topology,  SAW  trajectory,  and 
generate  the  sensors  measurements  made  of  the  SAW.  As 
aforementioned,  the  user  has  the  option  to  generate  new  sensor 
measurements  or  reuse  previously  generated  sensor  measurements. 

•  local_predict.m:  Executes  the  state  and  covariance 
prediction  using  the  equations  7  and  8  shown  in  Chapter  IV. 

•  local_update.m:  Executes  the  state  and  covariance  update 
using  the  equations  9  and  10  shown  in  Chapter  IV 

•  set_net_topology.m:  Constructs  the  communications  links 
among  sensors.  In  this  research,  each  sensor  is  connected 
to  all  other  sensors  in  the  network. 

•  communicate.m:  Communicates  information  between  all 
the  sensors. 

•  assimilate.m:  Adds  new  information  gained  from  sensors 
from  other  sensor  platfonns  while  discarding  common 
information. 

d.  Hybrid  Sensor  Network  Architecture  With  Information  Filter 
and  Track-to-Track  Fusion  Algorithm 

The  last  of  the  four  major  code  blocks,  this  code  block  implements  both 
data  and  track  fusion.  Its  setup  follows  that  of  the  previous  three  code  blocks.  The 
option  of  generating  new  sensor  measurements  or  using  previously  generated  sensor 
measurements  is  also  available  in  this  block.  The  functions  to  carry  out  data  fusion  using 
the  IF  are  the  same  as  those  used  for  the  decentralized  sensor  network,  namely, 
local_predict.m  and  local_update.m.  The  locally  fused  track  produced  by  each  sensor 
is  then  integrated  into  a  global  fused  track  using  the  track-to-track  fusion  algorithm 
(equations  1 1  and  12  shown  in  Chapter  IV)  residing  in  the  function  run_indep.m. 

3.  Evaluation  Parameters 

The  MOEs  are  derived  in  Chapter  II.  The  distributed  sensor  network  is  evaluated 
based  on  its  ability  to  provide  accurate  identification  and  early  warning  of  a  SAW 
intending  to  crash  into  Jurong  Island.  These  MOEs  are  related  to  the  probability  of 
impact  by  the  SAW  on  Jurong  estimated  as  the  SAW  is  being  tracked.  The  estimation  of 
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the  impact  probability  mirrors  the  computation  of  the  probability  of  destruction  of  a  point 
target  [47]  and  necessitates  the  computation  of  the  predicted  mean  impact  point  on 
Jurong  as  a  function  of  time. 


a.  Calculate  the  Mean  Impact  Point 


x(tgo|k-)= 


^xtgok'^ 

^tgok 

5W 

\^tgok  J 


At  time,  tk,  the  predicted  state  is,  '  '  ,  with  covariance 

P(tgo|k),  for  k=l,  ...,  N,  where  tgo  stands  for  time-to-go,  that  is,  the  difference  between 
the  predicted  time  the  SAW  would  strike  Jurong  and  the  current  time  at  which  its  state  is 
estimated.  The  predicted  uncertainty  in  the  x-component,  xtg0|k,  is  ox|k  =  [P(tgo|k)]n,  and 
the  predicted  uncertainty  in  the  y-component,  ytg0|k,  is  oy|k  =  [P(tgo|k)]33.  Hence,  the  x- 
component,  xtg0,  and  y-component,  ytg0,  of  the  predicted  mean  impact  point,  and  their 

2  2 

corresponding  uncertainties,  and  <TV  can  be  calculated  formulas  according  to 


Atgo 


(wTC„-‘x),  ^=o5>  (w'C,'Y> 


where 


W  =  (l  1  1  ...  1),  dim  W  =  N,  where  N  is  the  number  of  simulation  runs 

conducted, 


,  dimCx=NxN, 


,  dim  C„  =X  ■  N, 
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b.  Calculate  the  Probability  of  Impact 

Referencing  [47],  the  probability  density  function,  f(x,y),  for  the  projected 
impact  point  from  a  point  target  (on  Jurong  Island)  is  given  by 


f(x=y)=7^ 


•"Vu 


•exp 


where  ( x  tg0,  y  tg0)  are  the  mean  coordinates  and  <7^ 


2 


are  the  variances  of  f(x,y). 


The  probability  of  impact  within  a  circle  of  radius  R,  measured  from 
Jurong  Island,  can  be  calculated  from 


p  =  dxdy’ 


where  r=^«?  Thus, 


P  =  7^—  IU  exP 


•I3D 


(x-Staf  (yy«gi) 


2o?, 


dxdv. 


which  is  obtained  numerically. 


In  this  simulative  study,  it  is  observed  that  cr-  ftf  cr-  .  Hence,  making 

xtgo  >tgo 

the  approximation  of  cr-  =  a  ,  the  probability  of  impact,  P,  can  be  simplified 

xtgo  stgo 

to 


p=d?tU“p 


(**■}  0 


!&■ 


!&■ 


dxdv. 
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To  further  simplify  subsequent  calculations,  the  coordinate  axes  are 


rotated  so  that  r0  lies  along  the  positive  x-axis.  Then, 


2 

(*-ro) 


2^  20^ 


dxdv, 


which,  following  a  subsequent  transformation  to  polar  coordinates,  becomes 


KVi)=^(4)Ct^ 


2c1  <r  J 


Finally,  P  can  be  written  as 

p(!«)=s«*»h» 


where  g  and  hr  are  calculated  recursively  according  to 


*»=;fe)*.-i'with*» =expfi)' and 


h»=-S©  exp(-&)+hn-l.withh„  =  (^)j;1e-fe“du  =  1- 


R-1 

e  -a* , 


4.  Test  Cases 

Table  5  summarizes  the  various  test  sets  used  in  this  simulative  study.  A  total  of 
50  simulation  runs  are  made  for  each  case,  in  which  the  same  set  of  inputs  (i.e.,  sensor 
observations)  is  given.  Hence,  any  difference  in  the  results  emanates  solely  from  the 
choice  of  sensor-network  architecture  and  sensor-fusion  algorithm.  Note  that  the 
numbers  of  sensors  used  is  the  number  of  sensors  used  in  the  sensor  network,  as  indicated 
Table  5.  The  actual  number  of  sensors  whose  tracking  range  covers  the  sector  (i.e., 
Sector  73)  where  Jurong  Island  is  located  is  smaller.  For  example,  if  the  test  case 
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indicates  five  fixed  sensors  used,  only  three  fixed  sensors  have  coverage  over  Sector  73 
because  this  sector  is  out  of  sensor  range  for  two  of  the  fixed  sensors  located  at  the  east 
entrance  of  the  Singapore  Strait.  Similarly,  if  the  test  case  indicates  nine  sensors 
(comprising  five  fixed  and  four  mobile  sensors)  in  actuality,  only  seven  sensors  (i.e., 
three  fixed  and  four  mobile  sensors)  are  covering  Sector  73. 


Table  5.  Various  Cases  Used  in  This  Simulative  Study 


Case 

No.  of 
Sensors 
used* 

Sensor 

Network 

Architecture 

Algorithm 

Compared 

to 

Objective 

1 

5  FS 

C 

KF 

2 

5  FS 

C 

IF 

Case  1 

Validate  that  KF  and  IF 
produces  the  same  results 

3 

5  FS 

D 

IF 

Case  2 

Validate  that  C  and  D 
produces  the  same  results 
for  this  given  context. 
Thereafter,  it  is  immaterial 
whether  C  or  D  is  used. 

4 

9 

(5FS+4MS) 

C  or  D 

IF 

Case  2 

More  sensors  equates  to 
better  results 

5 

5  FS 

H 

IF  + 

Track- to- 

track 

fusion 

Case  2 

Track  fusion  produces 
poorer  results 

6 

9 

(5FS+4MS) 

Hybrid 

IF  + 

Track- to- 

track 

fusion 

Case  4 

Track  fusion  produces 
poorer  results  and  the  gap 
narrows  with  more  sensors 

Case  5 

More  sensors  equates  to 
better  results 

7 

5  FS 

C  or  D 
(subjected  to 
10%,  30%, 

and  50% 

data  loss) 

IF 

Data  loss  results  in  poorer 
results 

Case  5 

Data  loss  narrows  the 
perfonnance  gap  between 
centralized/decentralized 
and  hybrid  architectures. 

8 

8 

(5FS+3MS) 

Hybrid 

IF  + 

Track- to- 

track 

fusion 

The  number  of  sensors  used 
is  meant  to  reflect  the 
maximum  number  of 
sensors  if  sensor  failures 
are  accounted  for  (as 
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covered  under  sensor 

coverage  analysis  in 

Chapter  2). 

9 

8 

(5FS+3MS) 

C  or  D 
(subjected  to 
10%,  30%, 

and  50% 

data  loss) 

IF 

Case  8 

Data  loss  narrows  the 
perfonnance  gap  between 
centralized/decentralized 
and  hybrid  architectures. 

10 

Cases  8 

and  9  vs 
Cases  5 

and  7 

Accounting  for  sensor 
failures,  cases  8  and  9 
reflects  the  maximum 
number  of  sensors 

providing  sensor  coverage 
over  the  sector  in  which 
Jurong  Island  sits,  while 
cases  5  and  7  reflects  the 
minimum  number  of 

sensors.  The  comparison 
between  them  will  provide 
insights  on  the  performance 
gap- 

(*FS  -  Fixed  Sensors,  MS  -  Mobile  Sensors,  C  -  Centralized,  D  -  Decentralized,  FI  -  Hybrid) 


C.  DISCUSSION  OF  RESULTS 

1.  Comparison  Between  Kalman  and  Information  Filters 

Comparing  cases  1  and  2,  the  simulation  results  for  these  test  cases  validate  that 
the  KF  and  IF  produce  identical  results.  As  indicated  by  Figure  21,  the  KF  and  IF 
produce  the  same  probability  of  impact  and  estimated  time  error.  This  result  is  expected 
because  both  filters  are  algebraically  the  same,  with  the  IF  being  in  the  logarithmic  form 
of  the  KF.  However,  IF  has  some  computational  advantages  over  KF,  given  its  simplicity 
in  making  observation  updates  to  predictions  [48].  Taking  the  computational  advantages 
into  consideration,  the  IF  is  used  for  subsequent  test  cases  for  all  the  three  sensor  network 
architectures.  This  research,  however,  does  not  seek  to  quantify  and  compare  the 
computational  efficacy  of  the  two  filters. 
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Figure  2 1 .  Comparison  of  KF  and  IF  for  Centralized  Five-sensor  Network  Architecture 
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2.  Comparison  Between  Centralized  and  Decentralized  DSN 
Architectures 

As  shown  by  Figure  22,  the  centralized  and  decentralized  DSN  architectures, 
through  test  cases  2  and  3,  produce  the  same  probability  of  impact  and  estimated  time 
error.  These  results,  too,  come  as  no  surprise,  because  both  architectures  use  the  same 
data-fusion  algorithm  (i.e.,  IF)  to  process  the  same  sensor  observations.  In  the  context  of 
this  research  and  as  described  in  Chapter  III,  the  main  difference  between  these  two 
architectures  is  that  the  decentralized  sensor  network  has  more  fusion  centers  as 
redundancies.  For  the  subsequent  test  cases,  only  the  centralized  DSN  architecture  are 
used  for  the  simulation  runs,  since  the  centralized  and  decentralized  DSN  architectures 
produce  the  same  results.  It  should  be  noted  that  the  results  of  the  centralized  DSN 
architecture  are  representative  of  both  architectures. 


61 


s 

|  DJJ 

l  03 

!  u 


ant 


2D 


MriflynfhfMj 

. 

1  1  1  ' 

/ 

r  "  1  "  ■ 

- Centalzed 

■  Decentralized  ■ 

- 

j 

/ 

- 

- 

/ 

- 

- 

/ 

- 

/ 

/ 

I 

- 

/ 

a 

- 

_ 

_ 

/ 

/ 

i 

l 

1 

20  150 

The  fa  hfBd  (9) 

EmitafcuBiTireEgfiifafe  tnJTnjfi 


IDO 


ID 


Figure  22.  Comparison  of  Five-sensor  Centralized  and  Decentralized  DSN 

Architectures 
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3.  Comparison  Between  Different  Number  of  Sensors  for  Centralized 
DSN 

As  indicated  by  Figure  23,  the  centralized  DSN  architecture  with  nine  sensors 
provide  earlier  warning  of  an  impending  attack  with  a  lower  magnitude  of  error  in  time 
than  does  the  centralized  DSN  architecture  with  five  sensors.  Though  the  results  may 
seem  intuitive  (i.e.,  more  sensors  equal  better  performance),  they  are  true  only  because 
the  additional  mobile  sensors  (i.e.,  the  four  mobile  sensors  in  addition  to  the  live  fixed 
sensors)  are  assumed  to  have  specifications  similar  to  those  of  the  fixed  sensors.  If  their 
specifications  are  significantly  poorer,  more  sensors  would  produce  a  resultant  fused 
track  with  bigger  errors.  That  is,  in  this  case,  a  network  with  more  sensors  would  not 
necessarily  perform  better  than  a  network  with  fewer  sensors. 
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Figure  23.  Comparison  of  Number  of  Sensors  (Five  and  Nine)  for  a  Centralized  DSN 
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4.  Comparison  Between  Centralized  and  Hybrid  DSN  Architectures 
Using  Five  Sensors 

The  results  produced  using  the  centralized  and  hybrid  sensor  network 
architectures  for  five  fixed  sensors  are  compared  here  (i.e.,  Case  5  vs.  Case  2).  Figure  24 
indicates  that  the  centralized  DSN  architecture  outperfonns  the  hybrid  architecture.  For 
the  centralized  DSN  architecture,  the  probability  of  impact  increases  from  0%  (at  240 
seconds  to  impact)  to  100%  (at  80  seconds  to  impact),  compared  to  200  seconds  and  60 
seconds  for  the  hybrid  architecture.  However,  this  comparison  is  made  under  the 
condition  of  unlimited  communications  bandwidth.  In  reality,  bandwidth  is  limited,  and 
disseminating  loads  of  raw  sensor  data  within  the  centralized  and  decentralized  DSN, 
instead  of  fused-track  estimates  for  the  hybrid  sensor-network  architectures,  would 
usually  result  in  data  loss  or  latency.  Data  loss  will  be  discussed  later  in  this  chapter. 
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Figure  24.  Comparison  between  Five-sensor  Centralized  and  Hybrid  DSN 

Architectures 
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5. 


Comparison  Between  Centralized  and  Hybrid  DSN  Architectures 
Using  Nine  Sensors 


Using  more  sensors  of  comparable  specifications  (nine  in  this  case)  results  in  a 
widening  of  the  performance  gap  between  the  centralized  and  hybrid  sensor-network 
architectures.  This  widening  is  attributed  to  the  better  increase  in  performance  for  the 
centralized  architecture  with  more  sensors  as  compared  to  the  moderate  increase  in 
performance  for  the  hybrid  architecture.  Based  on  the  error  and  standard  deviation  of  the 
time-to-impact  estimate,  the  hybrid  architecture  using  the  track-to-track  fusion  algorithm 
produces  time  estimates  with  bigger  errors  initially  and  a  smaller  standard  deviation  as 
compared  to  the  centralized  architecture.  This  smaller  standard  deviation,  o,  is  the  result 
of  the  fused  track  having  a  smaller  variance  (computed  using  Equation  12),  which  results 
in  a  greater  rate  of  change  of  the  probability  of  impact. 
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Figure  25.  Comparison  between  Nine-sensor  Centralized  and  Hybrid  DSN 

Architectures 
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6.  Comparison  Between  Different  Number  of  Sensors  for  Hybrid  DSN 

As  shown  in  Figure  25,  the  improvement  in  employing  more  sensors  for  the 
hybrid  architecture  is  not  significant  compared  to  that  of  the  centralized  architecture. 
Comparing  Figures  23  and  26  shows  the  stark  contrast  in  improvement  for  the  different 
architectures  when  more  sensors  are  used.  These  results  show  that  increasing  the  number 
of  sensors  produces  varying  degrees  of  performance  improvement,  depending  on  the 
chosen  DSN  architecture  in  use. 
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Figure  26.  Comparison  of  Number  of  Sensors  (Five  and  Nine)  for  a  Hybrid  DSN 
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7.  Effects  of  Data  Loss  for  Centralized  and  Decentralized  DSN 

Architectures 

a.  Using  Five  Sensors 

The  dissemination  of  raw  sensor  data  takes  up  much  more 
communications  bandwidth  than  that  of  track  estimates.  Communication  bandwidth 
limitations  would  usually  result  in  data  loss  or  data  latency.  In  this  study,  varying 
degrees  of  data  loss  are  incorporated  to  simulate  the  conditions  of  distributing  a  high 
volume  of  data  under  limited  bandwidth  for  the  centralized  and  decentralized  sensor- 
network  architectures. 

Figure  27  shows  the  perfonnance  results  for  the  centralized/decentralized 
sensor-network  architectures  with  0%,  10%,  30%,  and  50%  of  data  loss,  and  for  the 
hybrid  sensor  network  architecture  using  five  sensors.  Data  loss  narrows  the 
perfonnance  gap  between  the  centralized/decentralized  and  hybrid  architectures.  Also 
noteworthy  is  that  while  the  hybrid  architectures  do  not  provide  as  early  a  warning,  its 
high  rate  of  change  of  probability  of  impact  with  respect  to  time  allows  it  to  achieve  an 
estimation  of  100%  probability  of  impact  at  almost  the  same  time  as  the  rest  of  the 
variants  in  the  centralized/decentralized  architecture. 
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Figure  27.  Comparison  of  the  Centralized  DSN  with  No  Data  Loss,  10%,  30%,  and 

50%  Data  Loss,  and  the  Flybrid  DSN  of  Five  Sensors 


b.  Using  Eight  Sensors 

Figure  1 1  in  Chapter  II  show  that  the  maximum  and  minimum  numbers  of 
sensors  monitoring  Jurong  Island  are  six  and  three,  respectively.  As  explained  in  Table  5, 
the  number  of  sensors  monitoring  Jurong  Island  is  two  fewer  than  the  total  sensors  used. 
Figure  27  shows  the  sensor-network  performance  for  the  lower  number  of  sensors  limit. 
In  Figure  28,  the  upper  number-of-sensors  limit,  determined  from  the  sensor  coverage 
analysis,  is  simulated.  The  results  indicate  that  with  more  sensors  employed,  the  wide 
performance  gap  between  the  centralized  and  hybrid  sensor  network  architectures 
remains  even  with  the  injection  of  data  loss. 

The  hybrid  architecture  in  Figure  28,  which  is  similar  to  Figure  27,  also 
generates  a  high  rate  of  change  of  probability  of  impact  with  respect  to  time  to  impact, 
allowing  it  to  achieve  100%  at  almost  the  same  time  or  even  earlier  (for  the  case  of  50% 
data  loss)  than  the  centralized  architecture. 
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Figure  28.  Comparison  of  the  Centralized  DSN  with  No  Data  Loss,  10%,  30%,  and 
50%  Data  Loss,  and  the  Flybrid  DSN  of  Eight  Sensors 


8.  Comparison  Between  Different  DSN  Architectures  and  Different 
Number  of  Sensors 

The  simulation  results  of  Figures  27  and  28  are  presented  together  in  Figure  29. 
Relating  it  to  the  sensor  coverage  analysis  done  in  Chapter  II,  the  best  and  worst  case 
performance  for  any  of  the  sensor  network  architectures  for  the  given  suite  of  sensors  can 
be  gleaned. 


73 


PrrirtritynftifBd 


1—8 — ft— a— a— a — a 


8  Senses  • 
8  Senses  • 
8  Senses  ■ 
8  Senses' 
-« —  8  Senses  • 
—  5  Senses  ■ 
6--  5  Sensos  ■ 
Eh-  5  Sensos  ■ 
^"5  Sensos' 
I-  5  Sensos  ■ 


■Cents  zed  wilt 
■Cents  led  witi 
Cents  led  wilt 
■Cents  led  witt 
Hytnd 

Cents  led  wilt 
Cents  led  wilt 
■Cents  led  wilt 
Cen tailed  wifi 
Hybod 


no  dee  less 
10%  data  loss 
30%  data  loss 
50%  data  loss 

no  daB  loss 
1054  data  loss 
3054  data  loss 
5054  data  loss 


IS) 

Tire  fa  trpxJ  (a) 


Figure  29.  Comparison  of  the  Centralized  DSN  with  No  Data  Loss,  10%,  30%,  and 
50%  Data  Loss,  and  the  Hybrid  DSN  of  Five  and  Eight  Sensors 


Table  6  extracts  some  salient  data  points  from  Figure  29  to  illustrate  the  response 
time  from  the  point  when  the  probability  of  impact  breeches  0%,  50%,  80%,  or  90%,  and 
reaches  100%.  For  example,  taking  the  case  of  90%  probability  of  impact,  for  the 
centralized  DSN  with  no  data  loss,  using  eight  sensors,  the  early  warning  (i.e.,  time  to 
impact)  is  raised  to  220  seconds,  as  opposed  to  160  seconds  if  only  five  sensors  are  used. 

Relating  these  results  to  the  MOEs,  in  the  worst-case  scenario,  a  five-sensor 
hybrid  DSN  architecture  can  afford  a  early  impact  warning  of  only  150  seconds,  while  in 
the  best-case  scenario,  a  no-data-loss,  eight-sensor,  centralized  or  decentralized  DSN 
architecture  can  afford  up  to  290  seconds.  If  the  identification  of  intent  “confidence 
threshold”  is  set  at  80%,  the  worst-case  result  is  95  seconds  (using  a  five-sensor  hybrid 
DSN  architecture),  as  compared  with  the  best  case  of  235  seconds  using  a  no-data-loss, 
eight-sensor,  centralized  or  decentralized  DSN  architecture. 
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Table  6.  A  Selection  of  Salient  Data  Points 
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D.  PERFORMANCE  EVALUATION 

The  simulation  results  reveal  that  a  centralized  or  decentralized  architecture 
generally  outperforms  the  hybrid  architecture.  This  is  attributed  to  the  hybrid 
architecture’s  larger  state  estimation  errors  which  is  inherent  to  track  fusion  as  compared 
to  data  fusion.  In  other  words,  the  fused  tracks  are  not  as  accurate  as  they  would  be,  if 
they  were  fonned  directly  from  the  sensor  measurements.  This  finding  is  consistent  with 
those  reported  by  Chen  et  al.  [49]  and  Rogers  [50]. 

In  addition  to  the  inherent  disadvantage  of  using  the  track-to-track  fusion 
algorithm,  the  centralized  and  decentralized  architectures  are  operating  under  “ideal” 
conditions.  By  injecting  a  simple  form  of  data  loss,  it  is  shown  that  the  performance  gap 
between  them  and  the  hybrid  architecture  can  be  narrowed.  Hence,  should  additional 
constraints  such  as  data  latency,  asynchronous  data  arrival,  and  multiple-target  tracking 
be  taken  into  consideration,  these  would  add  additional  layers  of  complexity  and 
significantly  reduce  the  perfonnance  advantage  that  the  centralized  and  decentralized 
architectures  have  now. 

Next,  to  detennine  if  a  centralized  or  decentralized  sensor  network  architecture 
performs  better,  they  can  be  differentiated  through  the  degree  of  data  loss  faced  by  each 
network.  As  there  are  more  communications  links  at  the  bottom  tier  for  the  decentralized 
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sensor  network  architecture,  it  can  be  presumed  to  suffer  from  higher  data  loss  compared 
to  the  centralized  sensor  network  architecture.  As  an  example,  if  the  decentralized  DSN 
suffers  from  30%  data  loss  while  the  centralized  DSN  suffers  from  10%  data  loss,  then 
from  the  results  presented  in  Figures  27  through  29,  the  perfonnance  of  a  centralized 
architecture  is  distinctly  better. 

Overall,  comparing  all  three  DSN  architectures,  the  centralized  DSN  architecture 
produces  the  best  performance  in  terms  of  being  accurate  and  early  in  identifying  a  SAW 
with  the  intent  of  striking  Jurong  Island.  This  performance  evaluation  is  only  valid  under 
the  assumptions  herein  and  that  the  centralize  architecture  is  vulnerable  to  single-point  of 
failure  (at  the  fusion  center)  is  not  a  factor. 

E.  SUMMARY 

This  chapter  covers  the  details  and  results  of  the  simulative  study.  Guided  by  the 
SoSADP,  the  modeling  considerations  and  parameters  are  drawn  from  the  analysis  made 
in  Chapters  II  and  III,  which  ensure  that  the  M&S  program  is  consistent  with  mission 
needs  and  operational  settings.  The  study  results  indicate  that  the  number  of  sensors  and 
the  type  of  sensor  network  architecture  are  key  in  providing  an  accurate  identification  of 
SAW  intent  and  early  warning.  Under  the  assumptions  made,  neither  the  KF  nor  IF  data 
fusion  algorithm  has  an  advantage.  In  addition,  data  fusion  is  proven  to  produce  better 
perfonnance  than  track  fusion  (i.e.,  track-to-track  fusion).  Finally,  in  the  presence  of  data 
loss,  the  centralized  architecture  perfonns  best,  followed  by  the  decentralized,  and  then 
the  hybrid. 
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VI.  CONCLUSION  AND  FUTURE  RESEARCH 


A.  INTRODUCTION 

This  chapter  summarizes  the  purpose  and  conduct  of  this  thesis  research,  recaps 
the  key  findings,  and  proposes  potential  areas  for  future  research. 

B.  RESEARCH  SUMMARY 

A  successful  terrorist  attack  using  a  ship  as  a  weapon  on  shore  infrastructure  in 
the  Malacca  and  Singapore  straits  would  bring  chaos  to  global  trade  since  these  straits 
carry  over  a  quarter  of  the  world’s  commerce  and  half  the  world’s  oil.  This  calamity 
must  be  prevented.  Towards  this  goal,  this  thesis  aims  at  developing  and  detennining  the 
best  distributed  sensor  network  (DSN)  architecture  and  to  implement  a  sensor  fusion 
algorithm  to  effect  an  accurate  identification  of  a  SAW  and  warn  of  a  potential  attack  on 
oil  and  chemical  terminals  at  the  earliest  possible  time. 

The  work  in  this  thesis  involves  the  application  of  the  System-of-Systems 
Architecture  Development  Process  (SoSADP)  for  designing  alternative  DSN 
architectures,  Kalman  and  infonnation  filters  for  SAW  tracking  and  sensor  data  fusion,  a 
track-to-track  fusion  algorithm,  and  a  Monte  Carlo  simulative  study  to  assess  the 
effectiveness  of  three  distributed  sensor- fusion  network  architectures:  centralized, 
decentralized,  and  hybrid.  All  DSN  architectures  include  the  various  sensors  that 
Singapore  deploys  in  and  along  the  Strait. 

The  results  and  concepts  in  this  thesis  provide  invaluable  insights  in  the 
development  of  response  capabilities  against  a  SAW  in  the  Singapore  Strait.  These 
results  and  concepts  can  also  be  extended  to  other  key  installations  within  Singapore  and 
other  countries. 

C.  KEY  FINDINGS 

The  simulative  study  results  (Figure  21)  indicate  that  the  Kalman  filter  and  the 
infonnation  filter  are  equally  effective  in  SAW  tracking.  Also,  given  the  assumption  that 
the  fusion  centers  employ  the  same  data-fusion  algorithm,  Figure  22  validates  that  the 
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centralized  and  decentralized  sensor  fusion  network  architectures  produce  the  same 
perfonnance  in  terms  of  early  and  accurate  identification  of  a  SAW.  Figures  23  and  26, 
show  proof  that  employing  more  sensors  would  deliver  better  results,  albeit  at  varying 
degrees,  for  all  three  DSN  architectures. 

Next,  Figure  29,  show  with  or  without  communications  bandwidth  limitation 
(implemented  in  the  form  of  data  loss),  a  ship  with  the  intent  to  attack  Jurong  can  be 
identified  accurately  at  an  earlier  time  with  the  centralized  and  decentralized  sensor- 
fusion  network  architectures  than  with  the  hybrid  architecture. 

Finally,  to  further  differentiate  the  perfonnance  of  the  centralized  and 
decentralized  sensor  networks,  the  decentralized  sensor  network  is  assumed  to  have  a 
higher  level  of  data  loss  due  to  more  communications  link  in  its  network  compared  to  the 
centralized  sensor  network.  With  all  the  assumptions  made,  the  centralized  DSN 
architecture  is  determined  to  perform  the  best,  followed  by  the  decentralized  DSN 
architecture,  and  then  the  hybrid  DSN  architecture. 

D.  AREAS  FOR  FUTURE  RESEARCH 

The  problem  scenario,  assumptions,  and  results  of  this  research  reflect  only  an 
instance  of  a  larger  and  more  complex  MDS  environment  and  MDA  problem.  All  of  the 
sensors,  sensor-platform  models,  and  threat  characteristics  created  for  this  research  can 
be  scaled  and  adapted  to  suit  other  mission  scenarios.  The  simulation  program  developed 
for  this  research  hopes  to  serve  as  a  launch  pad  for  the  implementation  of  multi-target 
scenarios,  other  data  fusion  algorithms,  such  as  the  particle  filter,  a  wider  variety  of 
sensors  and  sensor  platforms,  dynamic  sensor  management,  and  more  rigorous 
communications  bandwidth  constraints  (i.e.  data  latency  and  asynchronous  data).  The 
incorporation  of  budget  constraints  to  the  simulation  will  also  add  realism  to  the 
deployment  of  more  sensors  and  sensor  platforms. 

Accruing  from  the  results  produced  in  this  study,  one  pressing  area  of  future 
research  is  the  development  of  a  defensive  or  offensive  response  to  a  SAW  on  a  crash 
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course  towards  Jurong  Island.  Another  potential  area  is  the  integration  of  intelligence 
reports  and  open-source  databases  to  identify  a  SAW  far  from  the  shores  of  Singapore 
and  achieve  a  longer  response  time. 


E.  CONCLUSION 

This  thesis  apply  an  integrated  systems-engineering  methodology  to  develop  and 
detennine  the  best  distributed  sensor-network  architecture  for  a  given  sensor-fusion 
algorithm.  Given  the  context  and  assumptions  made,  the  key  research  findings  provide 
valuable  insights  to  the  Singapore  maritime  security  agencies  on  the  benefits  of 
employing  more  sensors,  the  advantages  of  using  the  centralized  DSN  architecture,  and 
the  limited  time  to  effect  a  response  to  against  a  SAW. 
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APPENDIX  A.  SENSOR  COVERAGE  ANALYSIS 


A.  MPA  SHORE  STATION  SENSOR  COVERAGE 

The  table  below  presents  the  estimated  number  of  MPA  sensor  platforms  covering 
each  sector  of  the  Singapore  Strait  for  each  hour  of  the  day.  These  platforms  are 
stationary.  A  total  of  five  platforms  are  used  in  this  research. 


*  TOD:  Time  of  the  Day 
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B. 


COASTAL  PATROL  CRAFT  SENSOR  COVERAGE 


The  table  below  presents  the  estimated  number  of  CPC  sensor  platform  covering 
each  sector  of  the  Singapore  Strait  for  each  hour  of  the  day.  Each  platform  is  mobile  and 
takes  on  a  different  patrol  route.  Two  platforms  are  used  in  this  research. 


TOD* 

MOBILE  CPC  SE1 

NSOR1 

3LATF0RM 

SECTOR 

71 

72 

73 

81 

82 

83 

91 

92 

93 

94 

1 

1 

1 

2 

1 

1 

3 

1 

1 

4 

1 

1 

5 

1 

6 

1 

7 

1 

1 

8 

1 

1 

9 

1 

1 

10 

1 

1 

11 

1 

1 

12 

1 

1 

13 

1 

1 

14 

1 

15 

1 

1 

16 

1 

1 

17 

1 

1 

18 

1 

1 

19 

1 

1 

20 

1 

1 

21 

1 

22 

1 

23 

1 

1 

24 

1 

1 

OD:  Time  of  the  Day 
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C.  PATROL  VESSEL  SENSOR  COVERAGE 

The  table  below  presents  the  estimated  PV  sensor  platfonn  covering  each  sector 
of  the  Singapore  Strait  for  each  hour  of  the  day,  given  its  assumed  patrol  route. 


TOD* 

MOBILE  CPC  SE1 

NSOR 1 

r'LATFORM 

SECTOR 

71 

72 

73 

81 

82 

83 

91 

92 

93 

94 

1 

1 

2 

1 

3 

1 

4 

1 

5 

1 

6 

1 

7 

1 

8 

1 

9 

1 

10 

1 

11 

1 

12 

1 

13 

1 

14 

1 

15 

1 

16 

1 

17 

1 

18 

1 

19 

1 

20 

1 

21 

1 

22 

1 

23 

1 

24 

1 

* 

fOD:  Time  of  the  Day 
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D.  MARITIME  PATROL  AIRCRAFT  SENSOR  COVERAGE 


The  table  below  presents  the  estimated  MPaA  sensor  platform  covering  each 
sector  of  the  Singapore  Strait  for  each  hour  of  the  day.  The  aircraft  is  assumed  to  patrol 
the  full  Singapore  Strait  several  times  within  an  hour. 


TOD* 

MOBILE  CPC  SE1 

NSOR 1 

3LATF0RM 

SECTOR 

71 

72 

73 

81 

82 

83 

91 

92 

93 

94 

1 

2 

3 

4 

5 

6 

7 

8 

9 

1 

10 

1 

11 

12 

13 

14 

15 

16 

17 

18 

19 

20 

1 

21 

22 

23 

24 

*  TOD:  Time  of  the  Day 
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E.  COMPLETE  SENSOR  COVERAGE  IN  THE  SINGAPORE  STRAIT 
1.  Case  Without  Sensor  Failures 

The  table  below  presents  the  total  number  of  sensor  platforms  covering  each 
sector  of  the  Singapore  Strait  for  each  hour  of  the  day,  assuming  there  are  no  sensor- 
platform  failures. 


TOD* 

TOTAL  SENSOR  PLATFORM 

IS 

SECTOR 

71 

72 

73 

81 

82 

83 

91 

92 

93 

94 

1 

3 

2 

3 

4 

6 

5 

3 

2 

1 

0 

2 

3 

4 

3 

4 

5 

5 

4 

2 

1 

0 

3 

2 

4 

5 

4 

5 

4 

4 

3 

1 

0 

4 

2 

3 

5 

6 

5 

4 

3 

3 

2 

0 

5 

1 

3 

4 

6 

7 

4 

3 

2 

2 

1 

6 

1 

2 

5 

6 

6 

5 

3 

3 

2 

0 

7 

1 

3 

4 

5 

6 

5 

5 

3 

1 

0 

8 

2 

3 

3 

4 

6 

6 

5 

3 

1 

0 

9 

3 

3 

4 

5 

7 

7 

5 

4 

3 

1 

3 

4 

4 

5 

6 

6 

6 

4 

3 

2 

11 

1 

3 

4 

4 

5 

4 

4 

4 

2 

1 

12 

1 

2 

4 

5 

5 

4 

3 

3 

3 

1 

13 

1 

2 

3 

5 

6 

4 

3 

2 

2 

2 

14 

1 

2 

4 

5 

5 

4 

3 

3 

3 

1 

15 

1 

3 

4 

4 

5 

4 

4 

4 

2 

1 

16 

2 

3 

3 

4 

5 

5 

5 

3 

2 

1 

17 

2 

2 

3 

4 

6 

6 

4 

3 

2 

0 

18 

2 

3 

3 

4 

6 

6 

5 

3 

1 

0 

19 

1 

3 

4 

5 

6 

5 

5 

3 

1 

0 

20 

2 

3 

6 

7 

7 

6 

4 

4 

3 

1 

21 

1 

3 

4 

6 

7 

4 

3 

2 

2 

1 

22 

2 

3 

5 

6 

5 

4 

3 

3 

2 

0 

23 

2 

4 

5 

4 

5 

4 

4 

3 

1 

0 

24 

3 

4 

3 

4 

5 

5 

4 

2 

1 

0 

MIN 

1 

2 

3 

4 

5 

4 

3 

2 

1 

0 

MAX 

3 

4 

6 

7 

7 

7 

6 

4 

3 

2 

*  TOD:  Time  of  the  Day 


85 


2.  Case  With  Sensor  Failures 

Taking  into  consideration  random  sensor-platfonn  failures,  a  Monte  Carlo 
simulation  of  50  runs  over  a  duration  of  30  days  was  carried  out.  The  table  below 
presents  the  total  number  of  sensor  platforms  covering  each  sector  of  the  Singapore  Strait 
for  each  hour  of  the  day,  given  random  sensor-platfonn  failures  and  a  repair  turnaround 
time  of  two  hours. 


TOD* 

TOTAL  SENSOR  PLATFORM 

IS 

SECTOR 

71 

72 

73 

81 

82 

83 

91 

92 

93 

94 

1 

3 

2 

3 

4 

6 

5 

3 

2 

1 

0 

2 

3 

4 

3 

4 

5 

5 

4 

2 

1 

0 

3 

2 

4 

5 

4 

5 

4 

4 

3 

1 

0 

4 

2 

3 

5 

6 

5 

4 

3 

3 

2 

0 

5 

1 

3 

4 

6 

7 

4 

3 

2 

2 

1 

6 

1 

2 

5 

6 

6 

5 

3 

3 

2 

7 

1 

3 

4 

5 

6 

5 

5 

3 

1 

8 

2 

3 

3 

4 

6 

6 

5 

3 

1 

0 

9 

3 

3 

4 

5 

7 

7 

5 

4 

3 

1 

10 

3 

4 

4 

5 

6 

6 

6 

4 

3 

2 

11 

1 

3 

4 

4 

5 

4 

4 

4 

2 

1 

12 

1 

2 

4 

5 

5 

4 

3 

3 

3 

1 

13 

1 

2 

3 

5 

6 

4 

3 

2 

2 

2 

14 

1 

2 

4 

5 

5 

4 

3 

3 

3 

1 

15 

1 

3 

4 

4 

5 

4 

4 

4 

2 

1 

16 

2 

3 

3 

4 

5 

5 

5 

3 

2 

1 

17 

2 

2 

3 

4 

6 

6 

4 

3 

2 

0 

18 

2 

3 

3 

4 

6 

6 

5 

3 

1 

0 

19 

1 

3 

4 

5 

6 

5 

5 

3 

1 

0 

20 

2 

3 

6 

7 

7 

6 

4 

4 

3 

1 

21 

1 

3 

4 

6 

7 

4 

3 

2 

2 

1 

22 

2 

3 

5 

6 

5 

4 

3 

3 

2 

0 

23 

2 

4 

5 

4 

5 

4 

4 

3 

1 

0 

24 

3 

4 

3 

4 

5 

5 

4 

2 

1 

0 

MIN 

1 

2 

3 

4 

5 

4 

3 

2 

1 

0 

MAX 

3 

4 

6 

7 

7 

7 

6 

4 

3 

2 

*  TOD:  Time  of  the  Day 
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The  minimum  and  maximum  number  of  sensor  platforms  for  each  sector  is  shown 
graphically  below. 


a.  Sensor  Coverage  Simulation  Source  Code 

%  Store  the  sensor  coverage  data  in  the  file  "SensorCoverageData.mat" 
%  Variable  "allsensordata"  includes  both  fixed  and  mobile  sensors 
%  Variable  "fixedsensordata"  includes  only  fixed  sensors 
%  Variable  "mobilesensordata"  includes  only  mobile  sensors 

%  Import  the  Variables  into  the  Workspace 
load  SensorCoverageData 

%  Indicate  the  duration  of  simulation  (in  days) 

DurSim  =  30; 

for  s  =  1 : DurSim 

sensorsdatabase(l,s)  =  struct(’day',0); 

%  Create  new  variable  that  accounts  for  the  random  failures 
newallsensorsdata  =  allsensorsdata; 

%  Random  allocation  of  failures  to  the  sensors 

NumF  =  1 ;  %  Indicate  the  number  of  failures  over  a  day 

DurF  =  2;  %  Indicate  the  duration  of  failure  (i.e.  TAT)  in  hours 

for  i  =  1  :NumF 
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%  For  a  24-hour  period  (number  of  rows  for  the  variable) 

a(i)  =  randi([l,(24-DurF)+l]);  %Bounding  the  failure  to  within  the 


day 


%  For  10  sectors  of  coverage  (number  of  columns  for  the  variable) 
b(i)  =  randi([l,10]); 

%  Check  for  no  sensor  coverage  for  the  selection 
while  (newallsensorsdata(a(i),  b(i))  ==  0) 
a(i)  =  randi([l,(24-DurF)+l]); 
b(i)  =  randi([l,10]); 
end 

%  Account  for  the  failure  duration 
forj  =  l:DurF 

if  (newallsensorsdata(a(i)+(j-l)  ,  b(i))  ==  0) 
newallsensorsdata(a(i)+(j-l)  ,  b(i))  =  0; 
else 

newallsensorsdata(a(i)+(j-l)  ,  b(i))  =  newallsensorsdata(a(i)+(j- 
l),b(i))-l; 
end 
end 
end 

sensorsdatabase(l,s).day  =  newallsensorsdata; 
end 

%  Calculate  the  average  sensor  coverage  for  the  duration  of  simulation 
sensorsum  =  0; 
for  s  =  1  :DurSim 

sensorsum  =  sensorsum  +  sensorsdatabase(s).day; 
end 

sensonnean  =  sensorsum/DurSim; 
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APPENDIX  B.  SIMULATION  PROGRAM  SOURCE  CODE 


A.  BLOCK  1:  CENTRALIZED  DATA  FUSION  ARCHITECTURE  USING 
KALMAN  FILTER 


%  This  file  executes  the  Monte-Carlo  simulation 

%  Number  of  Monte-Carlo  simulation  runs 
totalruns  =  50; 

%  Initialise  variables 
sumfirstleftTSS(l  Totalruns)  =  NaN; 
sumfirstleftTSSntoJIHigh(l  Totalruns)  =  NaN; 
sumfirstleftTSSntoJIModerate(  1  Totalruns)  =  NaN; 
sumfirstleftPL(  1  Totalruns)  =  NaN; 
sumfirstleftPLnto  JIHigh(  1  Totalruns)  =  NaN; 
sumfirstleftPLntoJIModerate(l  Totalruns)  =  NaN; 

%  Execute  each  run  by  generating  new  observations  or  using  previous  ones 
%  Produce  the  fused  track  for  each  run 
%  Then  pool  the  tracks  together  for  analysis 
for  run  =1  Totalruns 

buf=sprintf('Run:  =%d',run);  disp(buf); 
example_sim;  %generate  new  observations 

datastore9(run).observations=observations;  %  store  observations  to  be  executed  by 
other  filters 

observations  =  datastore9(run).  observations; 
runfilt; 

sumfirstleftTSS(run)  =  firstleftTSS; 
sumfirstleftTSSntoJIHigh(run)  =  firstleftTSSntoJIHigh; 
sumfirstleftTSSntoJIModerate(run)  =  firstleftTSSntoJIModerate; 
sumfirstleftPL(run)  =  firstleftPL; 
sumfirstleftPLntoJIHigh(run)  =  firstleftPLntoJIHigh; 
sumfirstleftPLntoJIModerate(run)  =  firstleftPLntoJIModerate; 
sumkdata(run,:,:)  =  kdata; 
sumesttimesigma(run,l,:)  =  esttimesigma; 
lowest(run)  =  min(sumkdata(run,7,1000:MAX_TIME/10)); 
end 

%  Calculate  Probability  of  Impact 
Cxtemp  =  squeeze(sumkdata(:,8,:)); 

Cytemp  =  squeeze(sumkdata(:,9,:)); 

Xtemp  =  squeeze(sumkdata(:,10,:)); 
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Ytemp  =  squeeze(sumkdata(:,l  1,:)); 
probability  =  zeros(l,ntime); 

warning  off; 
for  1  =  1  :ntime 

Cx  =  diag(Cxtemp(:,l))*(180A2); 

Cy=  diag(Cxtemp(:,l))*(180A2); 

X  =  (Xtemp(:,l)-  302.1 144444)*  180; 

Y  =  (Ytemp(:,l)-  347.7716667)*  180; 

W  =  ones(totalruns,l); 
sigma_xbar_tgo  =  inv((W')*inv(Cx)*W); 
sigma_ybar_tgo  =  inv((W')*inv(Cy)*W); 
sigma  =  sqrt(sigma_xbar_tgo); 

xbar_tgo  =  sigma_xbar_tgo*(((W')*inv(Cx)*X));%  Calculate  mean  impact  pt 
ybar_tgo  =  sigma_ybar_tgo*(((W')*inv(Cy)*Y));%  Calculate  mean  impact  pt 
r_not  =  sqrt(xbar_tgoA2  +  ybar_tgoA2); 

R  =  300; 

g  =  zeros(l,100); 
h  =  zeros(l,100); 
probtemp  =  0; 

recur  =  1 ; 

g_not  =  exp(-(r_notA2)/(2*sigmaA2));  %  g_not 
h_not  =  1  -  exp(-(RA2)/(2*sigmaA2));  %  h  not 
probtempnot  =  g_not*h_not; 

g(l)  =  (l/l)*((r_notA2)/(2*sigmaA2))*g_not; 

h(l)  =  -(l/factorial(l))*(((RA2)/(2*sigmaA2))A(l))*exp(-(RA2)/(2*sigmaA2))  +  h_not; 
probtemp  =  probtempnot  +  g(l)*h(l); 

while  (recur  <=  100) 

g(recur+l)  =  (l/(recur+l))*((r_notA2)/(2*sigmaA2))*g(recur); 
h(recur+ 1 )  =  (- 1  /  factorial(recur+ 1  ))*  (((RA2)/ (2  *  sigmaA2))A(recur+ 1 ))  *  exp(- 

(RA2)/(2*sigmaA2))  +  h(recur); 

probtemp  =  probtemp  +  g(recur+l)*h(recur+l); 
recur  =  recur  +  1 ; 
end 

probability(l,l)  =  probtemp; 
end 

%  Initialise  variables 
count  1  =  0; 
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count2  =  0; 
count3  =  0; 
count4  =  0; 
count5  =  0; 
count6  =  0; 

SsumfirstleftTSS  =  0; 

Ssumfl  rstlcftTSSntoJIHigh  =  0; 

S  sum  fi  rstl  c  ftT  S  S  n  toJ  I M  odcratc  =  0; 

S  sum  fi  rstl  eft  PL  =  0; 

Ssumfl  rstlcftPLntoJIHigh  =  0; 

SsumfirstleftPLntoJIModerate  =  0; 

%  Take  the  average  of  the  pooled  tracks 
for  m  =  1  :totalruns 

if  ~isnan(sumfi  rstlcftTSS(m)) 
count  1  =  count  1+1; 

SsumfirstleftTSS=SsumfirstleftTSS+sumfirstleftTSS(m); 

end 

if  ~isnan(sumfirstleftTSSntoJIHigh(m)) 
count2  =  count2+l; 

SsumfirstleftTSSntoJIHigh=SsumfirstleftTSSntoJIHigh+sumfirstleftTSSntoJIHigh(m); 

end 

if  ~isnan(sumfirstleftTSSntoJIModerate(m)) 
count3  =  count3+l; 

Ssumfl  rstlcftTSSntoJIModeratc=Ssum  (i  rstlcftTSSntoJIModeratc+sum  (i  rstlcftTSSntoJIM 
oderate(m); 
end 

if  ~i  snan(  sum  fi  rstl  c  f'tP  L( m ) ) 
count4  =  count4+l; 

SsumfirstleftPL=SsumfirstleftPL+sumfirstleftPL(m); 

end 

if  ~isnan(sumfirstleftPLntoJIHigh(m)) 
count5  =  count5+l; 

SsumfirstleftPLntoJIHigh=SsumfirstleftPLntoJIHigh+sumfirstleftPLntoJIHigh(m); 

end 

if  ~i  snan(sum  fi  rstl  c  f'tP  Ln  to  J I M  oderatc(m )) 
count6  =  count6+l; 
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SsumfirstleftPLntoJIModerate=SsumfirstleftPLntoJIModerate+sumfirstleftPLntoJIModer 

ate(m); 

end 

end 

m  can  llrstlc  UTSS  =  SsumllrstlcftTSS/count  1 ; 

meanfirstleftTSSntoJIHigh  =  SsumfirstleftTSSntoJIHigh/count2; 

meanfirstleftTSSntoJIModerate  =  SsumfirstleftTSSntoJIModerate/count3; 

mean  firstleftPL  =  SsumfirstleftPL/count4; 

m  can  11  rstl  c  ft  P  Ln  to  J I H  i  gh  =  SsumfirstleftPLntoJIHigh/count5; 

mean  nrstlcftPLntoJ  I  Moderate  =  SsumfirstleftPLntoJIModerate/count6; 

meantruetimedata  =  mean(sumkdata(:,l,:)); 

meandistuncert  =  mean(sumkdata(:,2,:)); 

meandistapart  =  mean(sumkdata(:,3,0); 

meanesttimedata  =  mean(sumkdata(:,6,:)); 

%  Calculate  the  sample  variance  of  the  estimated  time  to  impact 
for  p  =  1  :ntime 
sumtimediff=0; 
timediff=0; 
for  q  =  1 :  total  runs 

timediff  =  (sumkdata(q,6,p)-meanesttimedata(p))A2; 
sumtimediff=sumtimediff+timediff; 
end 

meansampletimesigma(p)  =  sqrt(sumtimediff  /(totalruns-1)); 
end 

warning  on; 

figure(30); 

elf; 

plot(meantruetimedata(:)',meandistuncert(:)','b',meantruetimedata(:)',meandistapart(:)Vg’) 
legend('Distance  Unccrtainty',’Di stance  Apart'); 
buf=sprintf(’Estimated  End  Point'); 
title(buf) 

xlim([min(lowest)*DT  MAX  TIME- 1  *DT]) 
ylim(  [0  1000]) 
xlabel('Simulated  Time  (s)') 
ylabel('Distance  (m)') 

figure(31); 

elf; 

plot(meantruetimedata(:)',abs((MAX_TIME-meantruetimedata(:)')- 
meanesttimedata(:)'),'b’,meantruetimedata(:)',meansampletimesigma,'r' ) 
legend('Estimated  Error’,'Standard  Deviation’); 
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buf=sprintf('Error  between  Time  Estimate  and  Truth'); 
title(buf) 

xlim([min(lowest)*DT  MAX  TIME- 1  *DT]) 
ylim(  [0  100]) 

xlabel(’Simulated  Time  (s)') 
ylabel('Time  (s)') 

figure(32); 

elf; 

plot(meantruetimedata(:)',probability(:),’k’) 
legend('Probability  of  Impact’); 
buf=sprintf('Probability  of  Impact’); 
title(buf) 

xlim([min(lowest)*DT  MAX  TIME- 1  *DT]) 
ylim([0  1]) 

xlabel(’Simulated  Time  (s)') 
ylabel(’Probability  of  Impact') 


93 


%  This  file  executes  the  Monte-Carlo  simulation  and  incorporates  dataloss 
%  (i.e.  observations  loss)  due  to  communication  bandwidth  limitations 

%  Number  of  Monte-Carlo  simulation  runs 
totalruns  =10; 

%  Initialise  variables 
sumfirstleftTSS(l  Totalruns)  =  NaN; 
sumfirstleftTSSntoJIHigh(l  Totalruns)  =  NaN; 
sumfirstleftTSSntoJIModerate(l  Totalruns)  =  NaN; 
sumfirstleftPL(l  Totalruns)  =  NaN; 
sumfirstleftPLnto  JIHigh(  1  Totalruns)  =  NaN; 
sumfirstleftPLntoJIModerate(l  Totalruns)  =  NaN; 

%  Execute  each  run  by  generating  new  observations  or  using  previous  ones 
%  Inject  data  loss 

%  Produce  the  fused  track  for  each  run 
%  Then  pool  the  tracks  together  for  analysis 
for  run  =1  Totalruns 

buf=sprintf(’Run:  =%d',run);  disp(buf); 
example_sim;  %generate  new  observations 

observations  =  datastore(run). observations;  %  using  previous  observations 

%  Since  previous  observations  (with  loss)  is  used,  there  is  no  need  to 
%  re-inject  data  loss.  If  new  observations  (with  no  loss)  are  made,  the 
%  following  codes  are  necessary  to  inject  data  loss. 

% - 

%  %  simulate  data  loss  due  to  bandwidth  limitations 

%  fori  =1:10 

%  j  =  randi(MAX_TIME/DT- 1 060-3)+ 1 060; 

% 

%  datalossduration  =  randi(3); 

%  fork=  1:  datalossduration 

%  for  n  =  1  :NUM_PLATFORMS 

%  observations(n,j+k).report(2:5)  =99999; 

%  end 

%  end 

%  end 

%  datastoreloss(run).observations=observations; 

% - 


runfilt; 

sumfirstleftTSS(run)  =  firstleftTSS; 
sumfirstleftTSSntoJIHigh(run)  =  firstleftTSSntoJIHigh; 
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sumfirstleftTSSntoJIModerate(run)  =  firstleftTSSntoJIModerate; 
sumfl  rstlcftPL(run)  =  firstleftPL; 
sum  (1  rstl  c  HP  Ln  to  J I H  i  gh(run )  =  firstleftPLntoJIHigh; 
sumfirstleftPLntoJIModerate(run)  =  fi  rstl  c  ft  P  L  n  to  J I M  odcratc ; 
sumkdata(run,:,:)  =  kdata; 
sumesttimesigma(run,l,:)  =  esttimesigma; 
lowest(run)  =  min(sumkdata(run,7,1000:MAX_TIME/10)); 
end 

%  Calculate  Probability  of  Impact 
Cxtemp  =  squeeze(sumkdata(:,8,:)); 

Cytemp  =  squeeze(sumkdata(:,9,:)); 

Xtemp  =  squeeze(sumkdata(:,10,:)); 

Ytemp  =  squeeze(sumkdata(:,l  1,:)); 
probability  =  zeros(l,ntime); 

warning  off; 
for  1  =  1  :ntime 

Cx  =  diag(Cxtemp(:,l))*(180A2); 

Cy  =  diag(Cxtemp(:,l))*(180A2); 

X  =  (Xtemp(:,l)-  302.1 144444)*  180; 

Y  =  (Ytemp(:,l)-  347.7716667)*  180; 

W  =  ones(totalruns,l); 
sigma_xbar_tgo  =  inv((W')*inv(Cx)*W); 
sigma_ybar_tgo  =  inv((W')*inv(Cy)*W); 
sigma  =  sqrt(sigma_xbar_tgo); 

xbar_tgo  =  sigma_xbar_tgo*(((W')*inv(Cx)*X));%  Calculate  mean  impact  pt 
ybar_tgo  =  sigma_ybar_tgo*(((W')*inv(Cy)*Y));%  Calculate  mean  impact  pt 
r_not  =  sqrt(xbar_tgoA2  +  ybar_tgoA2); 

R  =  300; 

g  =  zeros(l,100); 
h  =  zeros(l,100); 
probtemp  =  0; 

recur  =  1 ; 

g_not  =  exp(-(r_notA2)/(2*sigmaA2));  %  g_not 
h_not  =  1  -  exp(-(RA2)/(2*sigmaA2));  %  h  not 
probtempnot  =  g_not*h_not; 

g(l)  =  (l/l)*((r_notA2)/(2*sigmaA2))*g_not; 

h(l)  =  -(l/factorial(l))*(((RA2)/(2*sigmaA2))A(l))*exp(-(RA2)/(2*sigmaA2))  +  h_not; 
probtemp  =  probtempnot  +  g(l)*h(l); 
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while  (recur  <=  100) 

g(recur+l)  =  (l/(recur+l))*((r_notA2)/(2*sigmaA2))*g(recur); 
h(recur+ 1 )  =  (- 1  /  factorial(recur+ 1  ))*  (((RA2)/ (2  *  sigmaA2))A(recur+ 1 ))  *  exp(- 

(RA2)/(2*sigmaA2))  +  h(recur); 

probtemp  =  probtemp  +  g(recur+l)*h(recur+l); 
recur  =  recur  +  1 ; 
end 

probability(l,l)  =  probtemp; 
end 

%  Initialise  variables 
count  1  =  0; 
count2  =  0; 
count3  =  0; 
count4  =  0; 
count5  =  0; 
count6  =  0; 

SsumfirstleftTSS  =  0; 

SsumfirstlcftTSSnloJlHigh  =  0; 

Ssum  (i  rstl  c  ftT  S  S  n  to.1 1 M  oderatc  =  0; 

Ssum  (i  rstlcftPL  =  0; 

SsumfirstleftPLntoJIHigh  =  0; 

Ssum  nrstlcftPLnto.il  Moderate  =  0; 

%  Take  the  average  of  the  pooled  tracks 
for  m  =  1  :totalruns 

if  ~isnan(sumfirstlcftTSS(m)) 
count  1  =  count  1+1; 

Ssumfi  rstlcftTSS=SsumfirstleftTSS+sum  (IrstlcftTSS(m); 
end 

if  ~isnan(sumfirstleftTSSntoJIHigh(m)) 
count2  =  count2+l; 

SsumfirstleftTSSntoJIHigh=SsumfirstleftTSSntoJIHigh+sumfirstleftTSSntoJIHigh(m); 

end 

if  ~isnan(sumfirstlcftTSSntoJIModcratc(m)) 
count3  =  count3+l; 

Ssumfi  rstlcftTSSntoJIModcratc=Ssum  (i  rstlcftTSSntoJIModeratc+sum  (i  rstleftTSSntoJIM 
oderate(m); 
end 
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if  ~isnan(sumfirstleftPL(m)) 
count4  =  count4+l; 

SsumfirstleftPL=SsumfirstleftPL+sumfirstleftPL(m); 

end 

if  ~isnan(  sum  (i  rstlcftPLntoJIHi  gh(m)) 
count5  =  count5+l; 

SsumfirstleftPLntoJIHigh=SsumfirstleftPLntoJIHigh+sumfirstleftPLntoJIHigh(m); 

end 

if  ~isnan(sumfirstleftPLntoJIModerate(m)) 
count6  =  count6+l; 

SsumfirstleftPLntoJIModerate=SsumfirstleftPLntoJIModerate+sumfirstleftPLntoJIModer 

ate(m); 

end 

end 

mean  fir stlc RTSS  =  SsumfirstlcftTSS/count  1 ; 

meanfirstleftTSSntoJIHigh  =  SsumfirstleftTSSntoJIHigh/count2; 

m  can  li  rstl  c  1  ITS  S  n  to  J I M  o  derate  =  SsumfirstleftTSSntoJIModerate/count3; 

mcanfirstlcftPL  =  Ssumfirstlcfi.PL/count4; 

meanfirstleftPLntoJIHigh  =  Ssum  firstleftP  LntoJ  1  High/count5 ; 

meanfirstleftPLntoJIModerate  =  SsumfirstleftPLntoJIModerate/count6; 

meantruetimedata  =  mean(sumkdata(:,l,:)); 

meandistuncert  =  mean(sumkdata(:,2,:)); 

meandistapart  =  mean(sumkdata(:,3,:)); 

meanesttimedata  =  mean(sumkdata(:,6,:)); 

%  Calculate  the  sample  variance  of  the  estimated  time  to  impact 
for  p  =  1  :ntime 
sumtimediff=0; 
timediff=0; 
for  q  =  1 :  total  runs 

timediff  =  (sumkdata(q,6,p)-meanesttimedata(p))A2; 
sumtimediff=sumtimediff+timediff; 
end 

meansampletimesigma(p)  =  sqrt(sumtimediff  /(totalruns-1)); 
end 

figure(30); 

elf; 

plot(meantruetimedata(:)',meandistuncert(:)Vb',meantruetimedata(:)',meandistapart(:)Vg') 
legend('Distance  Uncertainty',' Distance  Apart'); 
buf=sprintf(’Estimated  End  Point'); 
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title(buf) 

xlim([min(lowest)*DT  MAX  TIME- 1  *DT]) 
ylim([0  1000]) 
xlabel('Simulated  Time  (s)') 
ylabel(’Distance  (m)') 


figure(31); 

elf; 

plot(meantruetimedata(:)',abs((MAX_TIME-meantruetimedata(:)')- 
meanesttimedata(:)'),'b',meantruetimedata(:)',meansampletimesigma,'r' ) 
legend('Estimated  Error', 'Standard  Deviation’); 
buf=sprintf('Error  between  Time  Estimate  and  Truth'); 
title(buf) 

xlim([min(lowest)*DT  MAX  TIME- 1  *DT]) 
ylim(  [0  100]) 

xlabel(’Simulated  Time  (s)') 
ylabel('Time  (s)') 

figure(32); 

elf; 

plot(meantruetimedata(:)',probability(:),’k’) 
legend('Probability  of  Impact'); 
buf=sprintf('Probability  of  Impact'); 
title(buf) 

xlim([min(lowest)*DT  MAX  TIME- 1  *DT]) 
ylim([0  1]) 

xlabel('Simulated  Time  (s)') 
ylabel('Probability  of  Impact') 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  is  a  script  file  allows  for  multi-target  multi-sensor  tracking 

%  Define  and  initialise  global  variables 

globals; 

ginit; 

buf=sprintf('Generating  Target  Information.. Please  Waifin’);  disp(buf); 
%  Set  up  time  base  for  target  simulations 
times=0:DT  :MAX_TIME; 

%  Generate  all  target’s  attack  destinations 
destinations=generate_destinations(times); 

%  Generate  all  target  trajectories 
targets=generate_targets(times); 

%  Generate  platform  information 
platforms=generate_platforms(times); 

%  Assign  sensors  to  platforms 
sensors=assign_sensors(platforms); 

buf=sprintf('Generating  Observations. ..Please  Wait\n');  disp(buf); 

%  Generate  observations 

observations=generate_observations(sensors, platforms, targets, times); 
buf=sprintf('Displaying  InformationW);  disp(buf); 

%  Display  track  information 

post_sim(targets, observations, sensors, platforms, destinations); 

%  This  file  defines  the  global  variables 
%  General  information 

global  TSS  X;  %  Traffic  Separation  Scheme  x-coordinates 

global  TSS  Y;  %  Traffic  Separation  Scheme  y-coordinates 
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global  PL_X;  %  Port  Limits  x-coordinates 

global  PL  Y;  %  Port  Limits  y-coordinates 

global  DT ;  %  simulation  time-step 

global  MAX  TIME;  %  maximum  simulation  time 

%  Target  definitions 

global  TARGETTYPE;  %  defines  type  of  target  (xy  or  V  phi) 

global  NUM  TARGETS;  %  number  of  targets  to  be  simulated 

%  Platform  definitions 

global  PLATFORM;  %  Platform  data  [xO,  yO,  phiO,  vel] 
global  NUM  PLATFORMS;  %  number  of  platforms 
global  MAX_RANGE;  %  maximum  observable  range 
global  SIGMA  XDOT; 
global  SIGMA  YDOT; 

%  Destination  definitions 

global  DESTINATION;  %  Destination  data  [xO,  yO,  phiO,  vel] 
global  NUM_DESTINATIONS;%  number  of  destinations 

%  State  and  Sensor  definitions 

global  SIGMA  Q;  %  process  noise  standard  deviation  for  feature  model 

global  SIGMA_RANGE_MPA;  %  Range  observation  noise 

global  SIGMA  BEARING  MPA;  %  Bearing  observation  noise 

global  SIGMA_RANGE_MPAC;  %  Range  observation  noise 

global  SIGMA  BEARING  MPAC;  %  Bearing  observation  noise 

global  SIGMA_RANGE_PV;  %  Range  observation  noise 

global  SIGMA  BEARING  PV;  %  Bearing  observation  noise 

global  SIGMA  RANGE  PCG;  %  Range  observation  noise 

global  SIGMA  BEARING  PCG;  %  Bearing  observation  noise 

global  Rfilter  MPA;  %  observation  noise  covariance  for  MPA 

global  Rfilter  MPAC;  %  observation  noise  covariance  for  MPAC 

global  Rfilter  PV;  %  observation  noise  covariance  for  PV 

global  Rfilter  PCG;  %  observation  noise  covariance  for  PCG 

global  F;  %  state  transition  equation 

global  XSIZE;  %  state  dimension 

global  ZSIZE;  %  observation  dimension 

%  Results  "database" 

global  kdata; 

global  esttimesigma; 

global  startplot; 

global  firstleftTSS; 

global  firstleftTSSntoJIHigh; 

global  firstleftTSSntoJIModerate; 
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global  firstleftPL; 

global  fi  rstl  c  IIP  Ln to J I H  i  gli ; 

global  fi  rstl  c  HP  Ln  to  J I M  odcratc; 

%  This  file  initializes  the  global  variables 

%  General  polygon  for  Traffic  Separation  Scheme  (TSS) 

X  =  [738.9629;  700.8405;  503.0354;  343.3085;  301.0754;  188.7756;  142.3798; 
291.7565;  356.6971;  474.1785;  703.9221;  738.9538;  738.9629]; 

Y  =  [443.8223;  412.1040;  377.3690;  303.7337;  330.3691;  359.0929;  316.1193; 
230.0498;  289.3964;  351.7903;  379.3511;  408.0001;  443.8223]; 

TSSX  =  X; 

TSSY  =  Y; 

%  General  polygon  for  Port  Limits,  estimated  0.5  mile  beyond  the  TSS 
[PL_X,  PL_Y]  =  bufferm2(’xy',X,Y,5,'out'); 

MAX_TIME=1 1500;  %  simulation  duration 

DT=10;  %  simulation  time-step 

%  Target  definitions 

NUM_TARGETS=1 ;  %  number  of  targets  to  be  tracked 

%  Platfonn  definitions 
NUM_PLATFORMS=5; 


%  Destination  definitions 
NUM_DESTINATIONS=l ; 

%  State  and  Sensor  definitions 

SIGMA_RANGE_MPA=5;  %  0.5nm  range  error  equivalent 

SIGMA_BEARING_MPA=0.0525;  %  3  degrees  bearing  error  in  radians 
SIGMA_RANGE_MPAC=6;  %  0.6nm  range  error  equivalent 

SIGMA_BEARING_MPAC=0.0875;  %  5  degrees  bearing  error  in  radians 
SIGMA_RANGE_PV=3;  %  0.3nm  range  error  equivalent 

SIGMA_BEARING_PV=0.0525;  %  3  degrees  bearing  error  in  radians 
SIGMA_RANGE_PCG=4;  %  0.4nm  range  error  equivalent 

SIGMA_BEARING_PCG=0.0875;  %  5  degrees  bearing  error  in  radians 
Rfilter  MPA  =  [ SIGM A  RAN GE  MP AA2  0;0  S IGM A  RAN GE  MP AA2 ] ; 
RfilterMPAC  =  [SIGMA_RANGE_MPACA2  0;0  S  IGM  ARAN  GEMP  AC  A2  ] ; 
Rfilter  PV  =  [ S IGM A_RAN GE_P VA2  0;0  SIGMA  RANGE  P VA2] ; 
RfilterPCG  =  [  S  IGM  ARAN  GE_P  CG  A2  0;0  S  IGM  ARAN  GE_P  CG  A2  ] ; 
XSIZE=4;  %  number  of  state  components 

ZSIZE=2;  %  number  of  sensor  measurement  components 

SIGMA_XDOT=0. 00286;  %  std  dev  of  lknot  in  the  x-direction 
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SIGMA_YDOT=0. 00286; 


%  std  dev  of  lknot  in  the  y-direction 


%  Results  "database" 

kdata  =  [7  MAXTIME/DT+1]; 

function  [latb,lonb]  =  bufferm2(varargin)  %lat, Ion, dist, direction, npts,outputfonnat) 
%BUFFERM2  Computes  buffer  zone  around  a  polygon 

% 

%  [latbjonb]  =  bufferm2(lat, Ion, dist, direction) 

%  [latb,lonb]  =  bufferm2(lat, Ion, dist, direction, npts) 

%  [latb,lonb]  =  bufferm2(lat, Ion, dist, direction, npts, outputformat) 

%  [xb,  yb]  =  bufferm2('xy',x,y, dist, direction, npts, outputformat) 

% 

%  This  function  was  originally  designed  as  a  replacement  for  the  Mapping 
%  Toolbox  function  bufferm,  which  calculates  a  buffer  zone  around  a 
%  polygon.  The  original  bufferm  function  had  some  serious  bugs  that  could 
%  result  in  incorrect  buffer  results  and/or  errors,  and  was  also  very  slow. 

%  As  of  R2006b,  those  bugs  have  been  fixed.  Flowever,  this  version  still 
%  maintains  a  few  advantages  over  the  original: 

% 

%  -  Can  be  applied  to  polygons  in  either  geographical  space  (as  in 
%  bufferm)  or  in  cartesian  coordinates. 

% 

%  -  Better  treatment  of  polygon  holes.  The  original  function  simply 
%  filled  in  all  holes;  this  version  trims  or  pads  holes  according  to  the 
%  buffer  width  given. 

% 

%  Input  and  output  format  is  identical  to  bufferm  unless  the  'xy'  option  is 
%  specified,  so  it  can  be  used  interchangeably. 

% 

%  Input  variables: 

% 

%  lat:  Latitude  values  defining  the  polygon  to  be  buffered. 

%  This  can  be  either  a  NaN-delimited  vector,  or  a  cell 

%  array  containing  individual  polygonal  contours  (each  of 

%  which  is  a  vector).  External  contours  should  be  listed 

%  in  a  clockwise  direction,  and  internal  contours  (holes) 

%  in  a  counterclockwise  direction. 

% 

%  Ion:  Longitude  values  defining  the  polygon  to  be  buffered. 

%  Same  format  as  lat. 

% 

%  dist:  Width  of  buffer,  in  degrees  of  arc  along  the  surface 

%  (unless  'xy'  is  used,  in  which  case  units  correspond  to 

%  x-y  coordinates) 

% 
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%  direction:  ’in'  or  'out' 

% 

%  npts:  Number  of  points  used  to  contruct  the  circles  around 

%  each  polygon  vertex.  If  omitted,  default  is  13. 

% 

%  outputformat:  'vector'  (NaN-delimited  vectors),  'cutvector' 

%  (NaN-clipped  vectors  with  cuts  connecting  holes  to  the 

%  exterior  of  the  polygon),  or  'cell'  (cell  arrays  in 

%  which  each  element  of  the  cell  array  is  a  separate 

%  polygon),  defining  format  of  output.  If  omitted, 

%  default  is  'vector'. 

% 

%  'xy':  If  first  input  is  'xy',  then  data  will  be  assumed  to 

%  lie  on  a  cartesian  plane  rather  than  on  a  sphere.  Use 

%  x  and  y  coordinates  as  first  two  inputs  rather  than  lat 

%  and  Ion.  Units  of  x,  y,  and  distance  should  be  the 

%  same. 

% 

%  Output  variables: 

% 

%  latb:  Latitude  values  for  buffer  polygon 

% 

%  lonb:  Longitude  values  for  buffer  polygon 

% 

%  Example: 

% 

%  load  conus 

%  tol  =  0.1;  %  Tolerance  for  simplifying  polygon  outlines 
%  [reducedlat,  reducedlon]  =  reducem(gtlakelat,  gtlakelon,  tol); 

%  dist  =  1 ;  %  Buffer  distance  in  degrees 
%  [latb,  lonb]  =  bufferm2(reducedlat,  reducedlon,  dist,  'out'); 

%  figure('Renderer', ’painters') 

%  usamap({’MN’,'NY’}) 

%  geoshow(latb,  lonb,  'DisplayType',  'polygon',  'FaceColor',  'yellow') 
%  geoshow(gtlakelat,  gtlakelon,... 

%  'DisplayType',  'polygon',  'FaceColor',  'blue') 

%  geoshow(uslat,  uslon) 

%  geoshow(statelat,  statelon) 

% 

%  See  also: 

% 

%  bufferm,  polybool 
%  Copyright  2010  Kelly  Kearney 
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% - 

%  Check  input 
% - 


error(nargchk(3,7,nargin)); 

%  Detennine  if  geographic  or  cartesian 

if  ischar(varargin{  1 })  &&  strcmp(varargin{  1 },  'xy') 
geo  =  false; 

param  =  varargin(2:end); 
else 

geo  =  true; 
param  =  varargin; 
end 

%  Set  defaults  if  not  provided  as  input 
nparam  =  length(param); 
if  geo 

[lat,  Ion,  dist]  =  deal(param{l:3}); 
else 

[Ion,  lat,  dist]  =  deal(param{  1:3});  %  Ion  =  x,  lat  =  y  for  mental  clarity,  will  switch 
back  at  end 
end 

if  nparam  <  4 
direction  =  ’out’; 
else 

direction  =  param  {4}; 
end 

if  nparam  <  5 
npts  =  13; 
else 

npts  =  param{5}; 
end 

if  nparam  <  6 

outputformat  =  ’vector'; 
else 

outputformat  =  param  {6}; 
end 
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%  Check  format  and  dimensions  of  input 

if  ~ismember(direction,  {’in’,  ’out'}) 

error('Direction  must  be  either  ’’in"  or  "out".'); 
end 

if  ~ismember(outputformat,  {’vector’,  'cutvector',  ’cell’}) 
error('Unrecognized  output  format  flag.'); 
end 

if -isnumeric(dist)  ||  numel(dist)  >  1 
error(’Distance  must  be  a  scalar.’) 
end 

if  -isnumeric(npts)  ||  numel(npts)  >  1 

error('Number  of  points  must  be  a  scalar.') 
end 

if  iscell(lat) 

for  il  =  1  :numel(lat) 

if  ~isvector(lat{il})  ||  ~isvector(lon{il})  ||  ~isequal(length(lat{il}),  length(lon{il})) 
error('Lat  (or  x)  and  Ion  (or  y)  must  be  vectors  or  cells  of  vectors  with  identical 
dimensions’); 
end 

lat{il}  =  lat{il}(:); 
lon{il}  =  lon{il}(:); 
end 
else 

if -isvector(lat)  ||  -isvector(lon)  [j  ~isequal(length(lat),  length(lon)) 

error('Lat  (or  x)  and  Ion  (or  y)  must  be  vectors  or  cells  of  vectors  with  identical 
dimensions’); 
end 

lat  =  lat(:); 

Ion  =  lon(:); 
end 


%■ 


%  Split  polygon(s)  into 
%  separate  faces 


%■ 


if  iscell(lat) 

[lat,  Ion]  =  polyjoin(lat,  Ion);  %  In  case  multiple  faces  in  one  cell, 
end 
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[latcells,  loncells]  =  polysplit(lat,  Ion); 


% - 

%  Create  buffer  shapes 
% - 


plotflag  =  0; 

if  plotflag 

Plt.x  =  Ion; 

Plt.y  =  lat; 

end 

latcrall  =  cell(0); 
loncrall  =  cell(0); 

foripoly=  l:length(latcells) 

%  Circles  around  each  vertex 

if  geo 

[late,  lone]  =  calccircgeo(latcells{ipoly},  loncells  {ipoly},  dist,  npts); 
else 

[lone,  late]  =  calccirccart(loncells{  ipoly},  latcells  {ipoly},  dist,  npts); 
end 

%  Rectangles  around  each  edge 
if  geo 

[latr,  lonr]  =  calcrecgeo(latcells  {ipoly},  loncells  {ipoly},  dist); 
else 

[lonr,  latr]  =  calcreccart(loncells  {ipoly},  latcells  {ipoly},  dist); 
end 

%  Union  of  circles  and  rectangles 

if  plotflag 

Plt.rectx  =  lonr; 

Plt.recty  =  latr; 

Plt.circx  =  lone; 

Plt.circy  =  late; 
end 
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[late,  lone]  =  multipolyunion(latc,  lone); 

[latr,  lonr]  =  multipolyunion(latr,  lonr); 

if  plotflag 

Plt.rectcombox  =  lonr; 

Plt.rectcomboy  =  latr; 

Plt.circcombox  =  lone; 

Plt.circcomboy  =  late; 
end 

[loner,  later]  =  polybool(’+',  lonr,  latr,  lone,  late); 

%  Union  of  new  circle/rectangle  combo  with  that  from  other  faces 

[loncrall,  latcrall]  =  polybool('+’,  loncrall,  latcrall,  loner,  later); 

%  Plotting  (for  debugging  only) 

if  plotflag 

Plt.allx  =  loncrall; 

Pit.  ally  =  latcrall; 

if  ipoly  ==  1 
figure; 

plot(Plt.x,  Plt.y,  ’k’,  ’linewidth',  2); 
hold  on 
end 

plot(cat(2,  Plt.rectx):}),  cat(2,  Plt.recty]:}),  ’b’); 
plot(cat(2,  Plt.circx]:}),  cat(2,  Plt.circy { : }),  ’r'); 
plot(Plt.allx  { 1 } ,  Plt.ally { 1 } ,  'g',  'linewidth',  2); 

end 


% - 

%  Calculate  union/difference 
% - 


switch  direction 
case  'out' 

[lonb,  latb]  =  polybool('+',  loncells,  latcells,  loncrall,  latcrall); 
case  'in' 
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[lonb,  latb]  =  polybool('-’,  loncells,  latcells,  loncrall,  latcrall); 
end 

if  plotflag 

[Plt.yfinal,  Plt.xfinal]  =  polyjoin(latb,  lonb); 

plot(Plt.xfinal,  Plt.yfinal,  'linestyle', ’color’,  [0  .5  0],  ’linewidth’,  2); 
end 


% - 

%  Reformat  output 
% - 


if  ~geo 

y  =  latb;  %  Switch,  since  cartesion  uses  opposite  order 
x  = lonb; 
latb  =  x; 
lonb  =  y; 
end 

switch  outputformat 
case  ’vector' 

[latb,  lonb]  =  polyjoin(latb,  lonb); 
case  ’cutvector’ 

[latb,  lonb]  =  polycut(latb,  lonb); 
case  ’cell’ 
end 


**** 

function  [late,  lone]  =  calccircgeo(lat,  Ion,  radius,  npts) 
%  lat  and  Ion:  n  x  1  vectors 
%  radius:  scalar 

radius  =  ones(length(lat),l)  *  radius; 

[late,  lone]  =  scirclel(lat,  Ion,  radius,  [],[],[],  npts); 
late  =  num2cell(latc,  1); 
lone  =  num2cell(lonc,  1); 

function  [latr,  lonr]  =  calcrecgeo(lat,  Ion,  halfwidth) 

%  lat  and  Ion:  n  x  1  vectors 
%  halfwidth:  scalar 

range  =  halfwidth  *  ones(length(lat)-l,  1); 
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az  =  azimuth(lat(l:end-l),  lon(l:end-l),  lat(2:end),  lon(2:end)); 

[latbl  1  ,lonbl  1  ]  =  reckon(lat(l:end-l),  lon(l:end-l),  range,  az-90); 
[latbrl,lonbrl]  =  reckon(lat(l:end-l),  lon(l:end-l),  range,  az+90); 
[latbl2,lonbl2]  =  reckon(lat(2:end),  lon(2:end),  range,  az-90); 
[latbr2,lonbr2]  =  reckon(lat(2:end),  lon(2:end),  range,  az+90); 

latr  =  [latbl  1  latbl2  latbr2  latbrl  latbl  1  ]'; 
lonr^  [lonbll  lonbl2  lonbr2  lonbrl  lonbll]'; 
latr  =  num2cell(latr,  1); 
lonr  =  num2cell(lonr,  1); 

function  [latu,  lonu]  =  multipolyunion(lat,  Ion) 

%  lat  and  Ion  are  n  x  1  cell  arrays  of  vectors 

latu  =  lat  { 1 } ; 
lonu  =  lon{  1 } ; 

for  ip  =  2:length(lat) 

[lonu,  latu]  =  polybool('+',  lonu,  latu,  Ion  {ip},  lat  {ip}); 
end 

[latu,  lonu]  =  polysplit(latu,  lonu); 


function  [xc,  yc]  =  calccirccart(x,  y,  radius,  npts) 

ang  =  linspace(0,  2*pi,  npts+1); 

ang  =  ang(end-l:-l:l); 

xc  =  bsxfun(@plus,  x,  radius  *  cos(ang)); 

yc  =  bsxfun(@plus,  y,  radius  *  sin(ang)); 

xc  =  num2cell(xc',  1); 

yc  =  num2cell(yc',  1); 

%  if  ~ispolycw(x,y) 

%  [xc,yc]  =  poly2ccw(xc,yc); 

%  end 

function  [xrec,  yrec]  =  calcreccart(x,  y,  halfwidth) 

dx  =  diff(x); 
dy  =  diff(y); 


isl  =  dx  >=  0  &  dy  >=  0; 
is2  =  dx  <  0  &  dy  >=  0; 
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is3  =  dx  <  0  &  dy  <  0; 
is4  =  dx  >=  0  &  dy  <  0; 

ishl  =  dy  ==  0  &  dx  >  0; 
ish2  =  dy  ==  0  &  dx  <  0; 


theta  =  zeros(5,l); 

theta(isl  |  is3)  =  atan(dy(isl  |  is3)./dx(isl  |  is3)); 
theta(is2  |  is4)  =  -atan(dy(is2  |  is4)./dx(is2  |  is4)); 

[xl,xr,yl,yr]  =  deal(zeros(size(dx))); 

xl(isl)  =  -halfwidth  *  sin(theta(isl)); 
xr(isl)  =  halfwidth  *  sin(theta(isl)); 
yl(isl)  =  halfwidth  *  cos(theta(isl)); 
yr(isl)  =  -halfwidth  *  cos(theta(isl)); 

xl(is2)  =  -halfwidth  *  sin(theta(is2)); 
xr(is2)  =  halfwidth  *  sin(theta(is2)); 
yl(is2)  =  -halfwidth  *  cos(theta(is2)); 
yr(is2)  =  halfwidth  *  cos(theta(is2)); 

xl(is3)  =  halfwidth  *  sin(theta(is3)); 
xr(is3)  =  -halfwidth  *  sin(theta(is3)); 
yl(is3)  =  -halfwidth  *  cos(theta(is3)); 
yr(is3)  =  halfwidth  *  cos(theta(is3)); 

xl(is4)  =  halfwidth  *  sin(theta(is4)); 
xr(is4)  =  -halfwidth  *  sin(theta(is4)); 
yl(is4)  =  halfwidth  *  cos(theta(is4)); 
yr(is4)  =  -halfwidth  *  cos(theta(is4)); 

xrec  =  [xl+x(l:end-l)  xl+x(2:end)  xr+x(2:end)  xr+x(l:end-l)  xl+x(l:end-l)]; 
yrec  =  [yl+y(l:end-l)  yl+y(2:end)  yr+y(2:end)  yr+y(l:end-l)  yl+y(l:end-l)]; 

xrec  =  num2cell(xrec,  2); 
yrec  =  num2cell(yrec,  2); 
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%  This  function  draws  a  circle  for  a  given  center  and  radius 
function  bubble  =  circle(center, radius) 

Resolution  =  100; 

THETA=linspace(0,2*pi, Resolution); 

RHO=ones(l  ,Resolution)*radius; 

[X,Y]  =  pol2cart(THETA,RHO); 
bubble.X=X+center(l); 
bubble.  Y=Y+center(2); 
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%  This  function  generates  the  SAW's  intended  destinations 
%  (i.e.  Areas  Al,  A2,  and  A3  of  Jurong  Island) 

function  destinations  =  generate  destinations(times) 

%  Define  the  global  variables 
globals; 

%  Define  the  destination  locations 
for  i=l  :NUM_DESTINATIONS 
%  Area  A 1 
if  i==l 

destinations(  1  ,i)  =  makedestination; 
destinations(  1  ,i).time=times(  1 ); 
destinations(  1  ,i)-id=i; 
destinations(l,i).x  =  302.1 144444; 
destinations(l,i)-y  =  347.7716667; 

%  Now  iterate  for  all  times 
[temp,ntimes]=size(times); 
for  n=2:ntimes 

destinations(n,i)=destination_step(destinations(n- 1  ,i),timcs(n)); 
end 
end 

%  Area  A2 
if  i==2 

destinations(  1  ,i)  =  makedestination; 
destinations(  1  ,i)  .time=times(  1 ); 
destinations(  1  ,i)-id=i; 
destinations(l,i).x  =  322.731 1  111; 
destinations(l,i)-y  =  367.21 16667; 

%  Now  iterate  for  all  times 
[temp  ,ntimes] =size(times) ; 
for  n=2:ntimes 

dcstinations(n,i)=dcstination_stcp(destinations(n-l  ,i),timcs(n)); 
end 
end 

%  Area  A3 
if  i==3 

destinations(  1  ,i)  =  makedestination; 
destinations(  1  ,i).timc=times(  1 ); 
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destinations(  1  ,i)-id=i; 

destinations(l,i)-x  =  343.3450000; 
destinations(l,i).y  =  382.5572222; 

%  Now  iterate  for  all  times 
[temp,ntimes]=size(times); 
for  n=2:ntimes 

destinations(n,i)=destination_step(destinations(n-l,i),times(n)); 

end 

end 

end 
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%  This  function  initialises  a  new  destination 


function  new=make_destination 

new=struct('id',0, 'time', 0,'x',0,'y',0, 'phi', 0,'vel',0, ’gamma’, 0); 
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%  This  function  is  to  step  a  destination 

function  next_destination=destination_step(destination,time) 
globals; 

%  This  is  a  no  motion  model 
dt=time-destination.time; 

nextdestination  =  makedestination;  %  space  for  destination  data  structure 

next_destination.id=destination.id; 

nextdestination.time  =  time; 

nextdestination.x  =  destination.x; 

nextdestination.y  =  destination.y; 

nextdestination.phi  =  destination.phi; 

nextdestination.vel  =  destination.vel; 

nextdestination.gamma  =  destination,  gamma; 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  generates  true  target  information  for  later  observation 
%  and  tracking.  Dynamics  of  target  are  contained  in  "target  step",  number 
%  targets  is  defined  by  the  globally  defined  variable  NUM_TARGETS.  A  data 
%  point  is  generated  at  each  of  a  set  of  "times.” 

function  targets  =  generate_targets(times) 

%  Define  the  global  variables 
globals; 

%  Define  a  SAW  at  pre-fixed  location  in  the  TSS 
for  i=l 

targets(l,i)  =  make_target; 
targets(  1  ,i).time=times(  1 ); 
targets(l,i).id=i; 
targets(l,i).x  =  738.9616667; 
targets(l,i).y  =  438.7050000; 
targets(  1 ,  i) .  gamma=0 ; 

%  Now  iterate  for  all  times 
[temp,ntimes]=size(times); 
for  n=2:ntimes 

targets(n,i)=target_step_SAW_path(targets(n- 1  ,i),times(n)); 
end 
end 

%  Create  non-SAW  contacts  at  random  locations  within  the  TSS 
%  Not  used  since  this  thesis  covers  a  single  target 
for  i=2:NUM_TARGETS 
targets(l,i)  =  make_target; 
targets(  1  ,i).time=times(  1); 
targets(l,i).id=i; 

x  =  TSS_X(l)*rand; 
y  =  TSS_Y(l)*rand; 

while  (inpolygon  (x,  y,TSS_X,  TSS_Y)  ~=  1) 
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x  =  TSS_X(l)*rand; 
y  =  TSS_Y(l)*rand; 
end 

targets(l,i)-x  =  x; 
targets(l,i).y  =  y; 
targets(l,i)-phi  =  2*pi*rand  -pi; 

targets(l,i).vel  =  MINTARGETVEL  +  (M  AX_T  ARGETVEL- 

MIN_TARGET_VEL)*rand; 
targets(l  ,i)-gamma=0; 

%  Now  iterate  for  all  times 
[temp,ntimes]=size(times); 
for  n=2:ntimes 

targets(n,i)=target_step(targets(n- 1  ,i),timcs(n)); 
end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  This  function  initialises  a  new  target 

function  new=make_target 

new=struct('id',0, 'time', 0,'x',0,'yC0, 'phi', 0,'vel',0, 'gamma', 0); 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  is  the  trajectory  taken  by  the  SAW 

function  next_target=target_step_SAW_path(target,time) 

globals; 

dt=time-target.time; 
nexttarget  =  maketarget; 
next_target.id=target.id; 
nexttarget.time  =  time; 

%  target.vel  =  0.042870370  (in  m/s  which  equates  to  15knots) 

%  target.vel  =  0.059160494  (in  m/s  which  equates  to  20knots) 

%  target.vel  =  0.060018519  (in  m/s  which  equates  to  21  knots) 

%  target.vel  =  0.062876543  (in  m/s  which  equates  to  22knots) 

if  (next  target.time  >=  0  &&  next  target.time  <  1086) 
next_target.x  =  target.x  +  dt*0.042870370*cos(-pi+0.720); 
next_target.y  =  target. y  +  dt*0.042870370*sin(-pi+0.720); 
next_target.phi  =  -pi+0.720; 
next_target.vel  =  0.042870370; 

elseif  (next_target.time  >=  1086  &&  next_target.time  <  5890) 
next_target.x  =  target.x  +  dt*0.042870370*cos(-pi+0.170); 
next_target.y  =  target.y  +  dt*0.042870370*sin(-pi+0.170); 
next_target.phi  =  -pi+0.170; 
next_target.vel  =  0.042870370; 

elseif  (next  target.time  >=  5890  &&  next  target.time  <  10199) 
next_target.x  =  target.x  +  dt*0.042870370*cos(-pi+0.430); 
next_target.y  =  target.y  +  dt*0.042870370*sin(-pi+0.430); 
next_target.phi  =  -pi+0.430; 
next_target.vel  =  0.042870370; 

elseif  (next  target.time  >=  10199  &&  next  target.time  <  1 1051) 
next_target.x  =  target.x  +  dt*0.042870370*cos(pi-0.770); 
next_target.y  =  target.y  +  dt*0.042870370*sin(pi-0.770); 
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next_target.phi  =  pi-0.770; 
next_target.vel  =  0.042870370; 

elseif  (next  target.time  >=  11051  &&  nexttarget.time  <  11500) 
nexttarget.x  =  target. x  +  dt*0. 059 160494*cos(pi- 1.360); 
next_target.y  =  target.y  +  dt*0. 059 160494*sin(pi- 1.360); 
nexttarget.phi  =  pi-1.360; 
nexttarget.vel  =  0.059160494; 

elseif  (next_target.time  >=  11500) 
next_target.x  =  target.x; 
next_target.y  =  target.y; 
nexttarget.phi  =  0; 
next_target.vel  =  0; 

end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  generates  trajectories  for  platforms 

function  platforms  =  generate_platforms(times) 

%  Define  the  global  variables 
globals; 

%  Define  5  fixed  platform  initial  locations 

%  Model  the  MPA  radar  stations 
for  i=l  :NUM_PLATFORMS 
if  i==l 

platforms(  l,i)  =  make_platform; 
platforms!  1  ,i).time=times(  1 ); 
platforms!  l,i).id=i; 
platforms!  l,i).x  =  285.4700000; 
platforms!  l,i).y  =  360.8527778; 
platforms! l,i).phi  =  2*pi*rand  -pi; 
platforms!  l,i).vel  =  0; 
platforms!  l,i).gamma=0; 

%  Now  iterate  for  all  times 
[temp,ntimes]=size(times); 
for  n=2:ntimes 

platforms(n,i)=platform_step(platforms(n- 1  ,i),times(n)); 
end 
end 

if  i==2 

platforms!  l,i)  =  make_platform; 
platforms!  1  ,i).time=times(  1 ); 
platforms!  l,i).id=i; 
platforms!  l,i).x  =  342.7972222; 
platforms!  l,i).y  =  311.9850000; 
platforms! l,i).phi  =  2*pi*rand  -pi; 
platforms!  l,i).vel  =  0; 
platforms!  l,i).gamma=0; 
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%  Now  iterate  for  all  times 
[temp,ntimes]=size(times); 
for  n=2:ntimes 

platforms(n,i)=platform_step(platforms(n- 1  ,i),times(n)); 
end 
end 

if  i==3 

platforms(l,i)  =  make_platform; 
platforms(l,i)-time=times(l); 
platforms(l,i)-id=i; 
platforms(l,i).x  =  413.1333333; 
platforms(l,i).y  =  349.8194444; 
platforms(l,i)-phi  =  2*pi*rand  -pi; 

platforms(l,i)-vel  =  0; 

platforms(l,i)-gamma=0; 

%  Now  iterate  for  all  times 
[temp  ,ntimes] =size(times) ; 
for  n=2:ntimes 

platforms(n,i)=platfonn_step(platfonns(n- 1 ,  i),ti  mcs(  n )); 
end 
end 

if  i==4 

platforms(l,i)  =  make_platform; 
platforms(  1  ,i).time=times(  1 ); 
platforms(  1  ,i)-id=i; 
platforms(  1  ,i).x  =  46 1 .476 1 1 1 1 ; 
platforms(  1  ,i).y  =  403 .426 1111; 
platforms(l,i)-phi  =  2*pi*rand  -pi; 
platforms(l,i)-vel  =  0; 
platforms(  1 ,  i) .  gamma=0 ; 

%  Now  iterate  for  all  times 
[temp  ,ntimes] =size(times) ; 
for  n=2:ntimes 

platforms(n,i)=platfonn_step(platforms(n- 1 ,  i  ),ti  rncsfn )); 
end 
end 

if  i==5 

platforms(  1  ,i)  =  make_platform; 
platforms(  1  ,i)  .time=times(  1 ); 
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platforms(l,i)-id=i; 
platforms(l,i)-x  =  517.0844444; 
platforms(l,i).y  =  421.9305556; 
platforms(l,i)-phi  =  2*pi*rand  -pi; 
platforms(l,i)-vel  =  0; 
platforms(  1  ,i)-gamma=0; 

%  Now  iterate  for  all  times 
[temp,ntimes]=size(times); 
for  n=2:ntimes 

platforms(n,i)=platfomi_step(platforms(n- 1 ,  i),ti  mes(  n )); 
end 
end 

%  Define  3  mobile  platform  initial  locations 

%  Model  the  CPC 
if  i==6 

platforms(l,i)  =  make_platform; 
platforms(  1  ,i).time=times(  1 ); 
platforms(  1  ,i)-id=i; 
platforms(l,i).x  =  474.1785056; 
platforms(l,i).y  =  351.7903000; 

platforms(l,i)-phi  =  2*pi*rand  -pi; 
platforms(l,i)-vel  =  0; 

targets(  1 ,  i) .  gamma=0 ; 

%  Now  iterate  for  all  times 
[temp  ,ntimes] =size(times) ; 
for  n=2:ntimes 

platforms(n,i)=platfonn_step_PCG  l_path(platforms(n- 1  ,i),timcs(n)); 
end 
end 

if  i— 7 

platforms(l,i)  =  make_platform; 
platforms(  1  ,i)-time=times(  1 ); 
platforms(  1  ,i)-id=i; 
platforms(l,i)-x  =  474.1785056; 
platforms(l,i).y  =  351.7903000; 
platforms(l,i)-phi  =  2*pi*rand  -pi; 
platforms(l,i)-vel  =  0; 
targets(  1 ,  i) .  gamma=0 ; 

%  Now  iterate  for  all  times 
[temp,ntimes]=size(times); 
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for  n=2:ntimes 

platforms(n,i)=platfomi_step_PCG2_path(platforais(n-l,i),times(n)); 

end 

end 

%  Model  the  PV 
if  i==8 

platforms(l,i)  =  make_platform; 
platforms(  1  ,i).time=times(  1 ); 
platforms(  1  ,i).id=i; 
platforms!  l,i).x  =  474.1785056; 
platforms!  l,i).y  =  351.7903000; 
platforms! l,i)-phi  =  2*pi*rand  -pi; 
platforms!  l,i)-vel  =  0; 
targets!  1  ,i).gamma=0; 

%  Now  iterate  for  all  times 
[temp,ntimes]=size(times); 
for  n=2:ntimes 

platforms(n,i)=platform_stcp_PV_path!platforms!n-l  ,i),timcs(n)); 
end 
end 

%  Given  the  speed  and  sensor  coverage  of  the  MPaA, 

%  its  surveying  of  the  small  TSS  can  be  modeled  as  a  "fixed"  sensor 
%  with  complete  of  the  TSS  during  its  operation. 

%  Model  the  MPaA 
if  i==9 

platforms!  1  ,i)  =  make_platfonn;  %  make  the  platform  data  structure 

platforms!  1  ,i)  .time=times(  1 ); 

platforms!  1  ,i)-id=i; 

platforms!  l,i).x  =  474.1785056; 

platforms!  l,i).y  =  351.7903000; 

platforms! l,i)  phi  =  2*pi*rand  -pi; 

platforms!  l,i)-vel  =  0; 

targets!  1  ,i)-gamma=0; 

%  Now  iterate  for  all  times 
[temp,ntimes]=size(times); 
for  n=2:ntimcs 

platforms(n,i)=platform_stcp_MPAC(platforms(n-l  ,i),tiincs!n)); 
end 
end 
end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  This  function  initialises  a  new  platfonu 

function  new=make_platform 

new=struct('id',0, 'time', 0,'x',0,'yC0, 'phi', 0,'vel',0, 'gamma', 0); 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  This  is  a  no  motion  platform  model  for  MPA  radar  station 

function  next_platform=platform_step(platform,time) 

globals; 

dt=time-plat  form,  time; 
next_platform  =  make_platfonn; 
next_platform.id=platform.id; 
next_platform.time  =  time; 
next_platform.x  =  platfonn.x; 
next_platform.y  =  platform.y; 
next_platform.phi  =  platform.phi; 
next_platform.vel  =  platform,  vel; 
next_platform. gamma  =  platform.gamma; 
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%  This  is  the  platform  motion  model  for  one  of  the  two  CPCs  for  a  day 
function  next_platfonn=platform_step_PCG  l_path(platform,time) 
globals; 

dt=time-platform.time; 
next_platfonn  =  make_platfonn; 
next_platfonn.id=platform.id; 
next_platfonn.time  =  time; 

if  (next_platform.time  >=  0) 
next_platfonn.x  =  474.1785056; 
next_platfonn.y  =  351.7903000; 
next_platfonn.phi  =  platform.phi; 
next_platfonn.vel  =  platform.vel; 
next_platfonn.  gamma  =  platform. gamma; 
end 


% - 

%  This  models  the  platform's  patrol  path.  However,  by  making  the  assumption 
%  that  the  sensors  have  extended  range  to  simplify  sensor  management,  this 
%  modeling  is  not  used  here.  This  will  be  useful  for  future  work  with  the 
%  inclusion  of  sensor  management. 

% 

%  if  (next_platfonn.time  >=  0  &&  next_platfonn.time  <  18000) 

%  next_platform.x  =  platform.x  +  dt*0.015061617*cos(l.  519574188-0. 5*pi); 
%  next_platfonn.y  =  platform.y  +  dt*0.015061617*sin(l.  519574188-0. 5*pi); 
%  next_platfonn.phi  =  1. 519574188-0. 5*pi; 

%  next_platfonn.vel  =  0.015061617; 

%  next_platfonn. gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  18000  &&  next_platform.time  <  32400) 

%  next_platform.x  =  platform.x  +  dt*0.025102695*cos(l.  519574188+0. 5*pi); 
%  next_platfonn.y  =  platform.y  +  dt*0.025102695*sin(l.  519574188+0. 5*pi); 
%  next_platfonn.phi  =  1.5 19574188+0. 5*pi; 

%  next_platfonn.vel  =  0.025 102695; 

%  next_platform.  gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  32400  &&  next_platform.time  <  46800) 

%  next_platfonn.x  =  platform.x  +  dt*0.015061617*cos(l.  5 19574188-0. 5*pi); 
%  next_platform.y  =  platform.y  +  dt*0.0 1506 1 6 1 7*sin(  1 . 519574188-0. 5*pi); 
%  %next_platform.phi  =  1. 519574188-0. 5*pi; 

%  %next_platform.vel  =  0.015061617; 
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%  %next_platform. gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  46800  &&  next_platform.time  <  61200) 

%  next_platform.x  =  platform.x  +  dt*0.025102695*cos(l.  519574188+0. 5*pi); 
%  next_platform.y  =  platform.y  +  dt*0.025102695*sin(l.  519574188+0. 5*pi); 
%  next_platform.phi  =  1. 519574188+0. 5*pi; 

%  next_platform.vel  =  0.025 102695; 

%  next_platform. gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  61200  &&  next_platform.time  <  75600) 

%  next_platfonn.x  =  platform.x  +  41*0.015061617*008(1. 5 19574188-0. 5*pi); 
%  next_platform.y  =  platform.y  +  dt*0.0 1 506 1 6 1 7*sin(  1.5195741 88-0.5  *pi); 
%  %next_platform.phi  =  1. 519574188-0. 5*pi; 

%  %next_platform.vel  =  0.015061617; 

%  %next_platform.  gamma  =  platform,  gamma; 

% 

%  elseif  (next_platform.time  >=  75600  &&  next_platform.time  <  86400) 

%  next_platform.x  =  platform.x  +  dt*0. 025 102695*cos(l.  5 19574188+0. 5*pi); 
%  next_platform.y  =  platform.y  +  dt*0.025102695*sin(l. 519574188+0. 5*pi); 
%  next_platform.phi  =  1. 519574188+0. 5*pi; 

%  next_platform.vel  =  0.025 102695; 

%  next_platform.gamma  =  platform,  gamma; 

% 

%  elseif  (next_platform.time  >=  86400) 

%  next_platform.x  =  platform.x; 

%  next_platform.y  =  platform.y; 

%  next_platform.phi  =  0; 

%  next_platform.vel  =  0; 

%  next_platform. gamma  =  0; 

%  end 

% - 
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%  This  is  the  platform  motion  model  for  one  of  the  two  CPCs  for  a  day 
function  next_platform=platform_step_PCG2_path(platform,time) 
globals; 

dt=time-platform.time; 
next_platfonn  =  make_platfonn; 
next_platfonn.id=platform.id; 
next_platfonn.time  =  time; 

if  (next_platform.time  >=  0) 
next_platfonn.x  =  474.1785056; 
next_platfonn.y  =  351.7903000; 
next_platfonn.phi  =  platform.phi; 
next_platfonn.vel  =  platform.vel; 
next_platfonn.  gamma  =  platform. gamma; 
end 


% - 

%  This  models  the  platform's  patrol  path.  However,  by  making  the  assumption 
%  that  the  sensors  have  extended  range  to  simplify  sensor  management,  this 
%  modeling  is  not  used  here.  This  will  be  useful  for  future  work  with  the 
%  inclusion  of  sensor  management. 

% 

%  if  (next_platfonn.time  >=  0  &&  next_platform.time  <  18000) 

%  next_platform.x  =  platform.x  +  dt*0.013140537*cos(-l. 330818664+0. 5*pi); 
%  next_platfonn.y  =  platform.y  +  dt*0.013140537*sin(-l. 330818664+0. 5*pi); 
%  next_platfonn.phi  =  - 1.3308 18664+0. 5  *pi; 

%  next_platfonn.vel  =  0.013 140537; 

%  next_platfonn. gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  18000  &&  next_platform.time  <  32400) 

%  next_platform.x  =  platform.x  +  dt*0. 02 1900894*cos(- 1.3308 18664-0. 5  *pi); 
%  next_platfonn.y  =  platform.y  +  dt*0. 02 1900894*sin(- 1.3308 18664-0. 5  *pi); 
%  next_platfonn.phi  =  -1. 330818664-0. 5*pi; 

%  next_platfonn.vel  =  0.021900894; 

%  next_platform.  gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  32400  &&  next_platform.time  <  46800) 

%  next_platfonn.x  =  platform.x  +  dt*0.013140537*cos(-l. 330818664+0. 5*pi); 
%  next_platform.y  =  platform.y  +  dt*0.013140537*sin(-l. 330818664+0. 5  *pi); 
%  next_platform.phi  =  - 1.3308 18664+0. 5  *pi; 

%  next_platfonn.vel  =  0.013 140537; 
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%  next_platform.  gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  46800  &&  next_platform.time  <  61200) 

%  next_platform.x  =  platform.x  +  dt*0. 02 1900894*cos(- 1.3308 18664-0. 5  *pi); 
%  next_platform.y  =  platform.y  +  dt*0. 02 1900894*sin(- 1.3308 18664-0. 5  *pi); 
%  next_platform.phi  =  - 1.3308 18664-0. 5  *pi; 

%  next_platform.vel  =  0.021900894; 

%  next_platform. gamma  =  platform,  gamma; 

% 

%  elseif  (next_platform.time  >=  61200  &&  next_platform.time  <  75600) 

%  next_platform.x  =  platform.x  +  dt*0.013140537*cos(-l.  330818664+0. 5*pi); 
%  next_platform.y  =  platform.y  +  dt*0.013140537*sin(-l. 330818664+0. 5*pi); 
%  next_platform.phi  =  - 1.3308 18664+0. 5  *pi; 

%  next_platfonn.vel  =  0.013 140537; 

%  next_platform.  gamma  =  platform,  gamma; 

% 

%  elseif  (next_platform.time  >=  75600  &&  next_platform.time  <  86400) 

%  next_platform.x  =  platform.x  +  dt*0.021900894*cos(-l. 330818664-0. 5*pi); 
%  next_platfonn.y  =  platform.y  +  dt*0. 02 1900894*sin(- 1.3308 18664-0. 5  *pi); 
%  next_platform.phi  =  -1. 330818664-0. 5*pi; 

%  next_platfonn.vel  =  0.02 1900894; 

%  next_platfonn.  gamma  =  platform,  gamma; 

% 

%  elseif  (next_platform.time  >=  86400) 

%  next_platfonn.x  =  platform.x; 

%  next_platform.y  =  platform.y; 

%  next_platform.phi  =  0; 

%  next_platform.vel  =  0; 

%  next_platform.  gamma  =  0; 

%  end 

% - 
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%  This  is  the  platform  motion  model  for  PV  for  a  day 
function  next_platform=platform_step_PV_path(platform,time) 
globals; 

dt=time-platform.time; 
next_platfonn  =  make_platfonn; 
next_platfonn.id=platform.id; 
next_platfonn.time  =  time; 

if  (next_platform.time  >=  0) 
next_platfonn.x  =  474.1785056; 
next_platfonn.y  =  351.7903000; 
next_platfonn.phi  =  platform.phi; 
next_platfonn.vel  =  platform.vel; 
next_platfonn.  gamma  =  platform. gamma; 
end 


% - 

%  This  models  the  platform's  patrol  path.  However,  by  making  the  assumption 
%  that  the  sensors  have  extended  range  to  simplify  sensor  management,  this 
%  modeling  is  not  used  here.  This  will  be  useful  for  future  work  with  the 
%  inclusion  of  sensor  management. 

% 

%  if  (next_platfonn.time  >=  0  &&  next_platform.time  <  43200) 

%  next_platform.x  =  platform.x  +  dt*0.013972407*cos(0. 1528 13367); 

%  next_platfonn.y  =  platform.y  +  dt*0.013972407*sin(0. 1528 13367); 

%  next_platfonn.phi  =  0.152813367; 

%  next_platfonn.vel  =  0.013972407; 

%  next_platfonn. gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  43200  &&  next_platform.time  <  86400) 

%  next_platform.x  =  platform.x  +  dt*0.013972407*cos(0.152813367+pi); 

%  next_platform.y  =  platform.y  +  dt*0.013972407*sin(0.152813367+pi); 

%  next_platfonn.phi  =  0.152813367+pi; 

%  next_platfonn.vel  =  0.013972407; 

%  next_platform.  gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  86400) 

%  next_platfonn.x  =  platform.x; 

%  next_platform.y  =  platform.y; 

%  next_platform.phi  =  0; 

%  next_platfonn.vel  =  0; 
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%  next_platform. gamma  =  0; 

%  end 

% - - - 
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%  This  is  the  platform  motion  model  for  MPaA 

function  next_platfonn=platform_step_MPAC(platfonn,time) 

globals; 

next_platfonn  =  make_platfonn; 
next_platfonn.id=platform.id; 
next_platfonn.time  =  time; 

if  (next_platform.time  >=  0) 
next_platfonn.x  =  474.1785056; 
next_platfonn.y  =  351.7903000; 
next_platfonn.phi  =  platform.phi; 
next_platfonn.vel  =  platform.vel; 
next_platfonn.  gamma  =  platform. gamma; 
end 


133 


%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  creates  and  assigns  sensors  to  platforms 
%  It  assigns  one  sensor  to  one  platform 

function  sensors=assign_sensors(platforms) 

globals; 

%  Define  sensors  for  the  5  fixed  platforms 

for  i=l  :NUM_PLATFORMS 
if  i==l 

sensors(i)=make_sensor; 

sensors(i).id=i; 

sensors(i).platform=i; 

sensors(i).r_err=SIGMA_RANGE_MPA; 

sensors(i).b_err=SIGMA_BEARING_MPA; 

sensors(i).point=0;  %  zero  point  angle 

sensors(i).beam_view=2*pi; 

sensors(i).max_range=154;  %  an  equivalent  of  15nm 


elseif  i==2 

sensors(i)=make_sensor; 

sensors(i).id=i; 

sensors(i).platform=i; 

sensors(i).r_err=SIGMA_RANGE_MPA; 

sensors(i).b_err=SIGMA_BEARING_MPA; 

sensors(i).point=0; 

sensors(i).beam_view=2*pi; 

sensors(i).max_range=l  54; 


elseif  i==3 

sensors(i)=make_sensor; 

sensors(i).id=i; 

sensors(i).platform=i; 

sensors(i).r_err=SIGMA_RANGE_MPA; 

sensors(i).b_err=SIGMA_BEARING_MPA; 

sensors(i).point=0; 
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sensors(i).beam_view=2*pi; 
sensors(i).max_range=l  54; 


elseif  i==4 

sensors(i)=make_sensor; 

sensors(i).id=i; 

sensors(i).platform=i; 

sensors(i).r_err=SIGMA_RANGE_MPA; 

sensors(i) .  b_err=S  IGM  A_BE  ARIN  G_MP  A ; 

sensors(i).point=0; 

sensors(i).beam_view=2*pi; 

sensors(i).max_range=l  54; 


elseif  i==5 

sensors(i)=make_sensor; 

sensors(i).id=i; 

sensors(i).platform=i; 

sensors(i) .  r_err=S  IGM  ARAN  GE_MP  A ; 

sensors(i).b_err=SIGMA_BEARING_MPA; 

sensors(i).point=0; 

sensors(i).beam_view=2*pi; 

sensors(i).max_range=l  54; 

%  Define  sensors  for  the  4  mobile  platforms 

%  Define  the  CPC  sensor 
elseif  i==6 

sensors(i)=make_sensor; 

sensors(i).id=i; 

sensors(i).platform=i; 

sensors(i).r_err=SIGMA_RANGE_PCG; 

sensors(i).b_err=SIGMA_BEARING_PCG; 

sensors(i).point=0; 

sensors(i).beam_view=2*pi; 

sensors(i).max_range=1000;  %  range  extended  to  simulate 
%  presence  at  Jurong  Island 

%  Define  the  CPC  sensor 
elseif  i==7 

sensors(i)=make_sensor; 

sensors(i).id=i; 

sensors(i).platform=i; 

sensors(i).r_err=SIGMA_RANGE_PCG; 

sensors(i).b_err=SIGMA_BEARING_PCG; 

sensors(i).point=0; 
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sensors(i).beam_view=2*pi; 

sensors(i).max_range=1000; 

%  Define  the  PV  sensor 
elseif  i==8 

sensors(i)=make_sensor; 

sensors(i).id=i; 

sensors(i).platform=i; 

sensors(i) .  r_err=S  IGM  A_RAN  GE_P V ; 

sensors(i).b_err=SIGMA_BEARING_PV; 

sensors(i).point=0; 

scnsors(i).beam_vicw=2*pi; 

sensors(i).max_range=1000; 

%  Define  the  MPaA  sensor 
elseif  i==9 

sensors(i)=make_sensor; 

sensors(i).id=i; 

sensors(i).platform=i; 

sensors(i) .  r_err=S  IGM  ARAN  GE_MP  AC ; 
sensors(i).b_err=SlGMA_BEARlNG_MPAC; 
sensors(i).point=0; 
sensors(i).beam_view=2*pi; 
sensors(i).max_range=1000; 
end 


136 


%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  This  function  initialises  a  new  sensor 

function  new=make_sensor 

new=struct('id',0, 'platform', 0,’r_err’,0,'b_err',0, 'point', 0,... 
'beam_view',0,'max_range’,0); 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  generates  observations  from  sensors  located  on 
%  platfonns  observing  targets  for  the  stipulated  time-steps 

function  observations=generate_observations(sensors, platforms, tracks, times) 

[temp,nsensors]=size(sensors); 
for  s=l:nsensors 
ntimes=l; 
for  t=times 

observations^, ntimes).sensor=s; 
observations^, ntimes).time=t; 

observations^, ntimes).report=sensor_report(sensors(s),  platforms,  tracks,  t); 
ntimes=ntimes+ 1 ; 
end 
end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  constructs  a  report  for  a  sensor  at  a  particular  time 
function  report=sensor_report(sensor, platforms, tracks, time) 
globals; 

report=zeros(NUM_TARGETS,5); 

%  Find  platform  location  at  this  time 
[px,py]=get_platform_loc(sensor  .platform, time, platforms); 

for  tnum=l  :NUM_TARGETS 
%  Find  target  location 
[tx,ty]=get_target_loc(tnum, time, tracks); 

%  Compute  range  and  bearing 

dx=tx-px; 

dy=ty-py; 

range=sqrt(dx*dx  +  dy*dy); 
bearing=atan2(dy,dx); 

%  Determine  if  this  is  actually  visable 
if  range  <  sensor.max  range 
%  Add  error  from  sensor  model 
report(tnum,  1  )=tnum; 

report(tnum,2)=range  +  scnsor.r_crr*(- 1  +2*rand); 
report(tnum,3)=bearing  +  sensor.b_err*(-l+25|!rand); 
report(tnum,4)=px; 
report(tnum,5)=py; 
else 

report(tnum,  1  )=tnum; 
report(tnum,2)=NaN; 
report(tnum,3)=NaN; 
report(tnum,4)=NaN ; 
report(tnum,  5  )=N  aN ; 
end 
end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  retrieves  the  location  of  the  platform 

function  [px,py]=get_platfonu_loc(pnum, time, platforms) 

[nsamps,nplats]=size(platforms); 
if  (pnum>  nplats)  |  (pnum  <1) 

error('Incorrect  platform  number  specified  in  get_platform_loc'); 
end 

if  (time<platforms(l,pnum).time)|(time>platforms(nsamps,pnum).time) 
error('Time  out  of  range  in  get_platform_loc'); 
end 

it=l; 

while  platforms(it, pnum). time  <  time 
it=it+l; 
end 

px=platforms(it,pnum).x; 
py=platforms(it,pnum)  .y; 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  retrieves  the  location  of  the  target 

function  [tx,ty]=get_target_loc(tnum, time, tracks) 

[nsamps,ntracks]=size(tracks); 
if  (tnum>  ntracks)  |  (tnum  <1) 
error('Incorrect  track  number  specified  in  get  target  loc'); 
end 

if  (time<tracks(  1 , tnum). time)|(time>tracks(nsamps, tnum). time) 
error('Time  out  of  range  in  get  target  loc'); 
end 

it=l; 

while  tracks(it, tnum). time  <  time 
it=it+l; 
end 

tx=tracks(it,tnum).x; 

ty=tracks(it,tnum).y; 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  displays  true  target  trajectories,  platfonn  trajectories, 

%  target  distinations,  and  sensor  observations  following  a  simulation  run 

function 

[true, plat, obs,dest]=post_sim(targets, observations, sensors, platforms, destinations) 
globals; 

%  Set  plotting  size 

[nsamps,nplatforms]=size(platforms); 

[nsamps,ndestinations]=size(destinations); 

[nsamps,ntargets]=size(targets); 

[nsensors,nsamps]=size(observations); 

figure(l) 

elf; 

hold  on 

data=zeros(2,nsamps); 
title(’True  Target  Motions') 
xlabel(’x -position  (m)’) 
ylabel(’y-position  (m)’) 

%  Plot  true  target  trajectories 
for  n=l:ntargets 
for  i=l:nsamps 
data(l  ,i)=targets(i,n).x; 
data(2,i)=targets(i,n).y; 
end 

true=plot(data(l,:),data(2,:),’b-’); 

end 

%  Plot  true  platform  trajectories 
for  n=l  mplatforms 
for  i=l:nsamps 
data(  1  ,i)=platforms(i,n).x; 
data(2,i)=platforms(i,n).y; 
end 
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if  n  ==  1 

plat=plot(data(  1 , : ),  data(2 , : ) ,  ’ko ') ; 
elseif  n  ==  2 

plat=plot(data(l,:),data(2,:),'ko'); 
elseif  n  ==  3 

plat=plot(data(l,:),data(2,:),'ko'); 
elseif  n  ==  4 

plat=plot(data(l,:),data(2,:),’ko’); 
elseif  n  ==  5 

plat=plot(data(l,:),data(2,:),'ko’); 
elseif  n  ==  6 

plat=plot(data(l,:),data(2,:),’r*'); 
elseif  n  ==  7 

plat=plot(data(l,:),data(2,:),'r*'); 
elseif  n  ==  8 

plat=plot(data(l,:),data(2,:),'gh'); 
elseif  n  ==  9 

plat=plot(data(l,:),data(2,:),’bo’); 

end 

end 

%  Plot  true  destination  locations 
for  n=l  mdestinations 
for  i=l:nsamps 
data(  1  ,i)=destinations(i,n).x; 
data(2,i)=destinations(i,n).y; 
end 

dest=plot(data(  1  ,:),data(2,:),'rh'); 
end 

%  Plot  sensor  observations 
for  n=l:nsensors  %  for  all  sensors 
for  i=l  :nsamps  %  for  all  time 
report=observations(n,i).report; 
for  m=l  :ntargets  %  for  all  targets 
if  report(m,l)  >  0  %  if  they  are  seen 

[zx,zy,R]=xy_obs(report,m,n);  %  convert  observation  to  global  xy  coordinates 
odata(l,m)=zx; 
odata(2,m)=zy; 
end 
end 

if  n  ==  1 

obs=plot(odata(l,:),odata(2,:),'k.'); 
elseif  n  ==  2 

obs=plot(odata(l,:),odata(2,:),’k.'); 
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elseif  n  ==  3 

obs=plot(odata(l,:),odata(2,:),’k.'); 
elseif  n  ==  4 

obs=plot(odata(l,:),odata(2,:),'k.'); 
elseif  n  ==  5 

obs=plot(odata(l,:),odata(2,:),’k.'); 
elseif  n  ==  6 

obs=plot(odata(l,:),odata(2,:),'r.'); 
elseif  n  ==  7 

obs=plot(odata(l,:),odata(2,:),’r.'); 
elseif  n  ==  8 

obs=plot(odata(l,:),odata(2,:),’g.'); 
elseif  n  ==  9 

obs=plot(odata(l,:),odata(2,:),’b.'); 

end 

end 

end 

legend([true, plat, obs,dest], ’Target  True  Position’/Tracking 

Observations’,'Intended  Target  Destination') 
hold  off 


Stations','Target 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  extract  an  xy  observation  from  a  range-bearing  report 

function  [zx,zy,R]=xy_obs(rep,n,sensor_id) 

globals; 

sr=sin(rep(n,3)); 
cr=cos(rep(n,3)); 
zx=rep(n,4)+rep(n,2)  *  cr; 
zy=rep(n,  5  )+rep(n,2)  *  sr; 

ROT=[cr  -sr;  sr  cr]; 
rangc2=rep(n,2)*rep(n,2); 
if  sensor_id==l 

sigma=[ SIGM A_RAN GE_MP AA2  0;0  range2*SIGMA_BEARING_MPAA2]; 
elseif  sensor_id==2 

sigma=  [ SIGM A_RAN GE_MP AA2  0;0  ran ge2 *SIGMA_BEARIN G_M P A A2] ; 
elseif  sensor_id==3 

sigma=  [ SIGM A_RAN GE_MP AA2  0;0  range2* SIGMA_BEARING_MPAA2] ; 
elseif  sensor_id==4 

sigma=  [ SIGM A_RAN GE_MP AA2  0;0  range2*SIGMA_BEARING_MPAA2]; 
elseif  sensor_id==5 

sigma=  [ SIGM A_RAN GE_MP AA2  0;0  range2*SIGMA_BEARING_MPAA2]; 
elseif  sensor_id==6 

sigma=  [ SIGM A_RAN GE_PC GA2  0;0  range2*SIGMA_BEARING_PCGA2]; 
elseif  sensor_id==7 

sigma=  [ SIGM A_RAN GE_PC G A2  0;0  range2*SIGMA_BEARING_PCGA2]; 
elseif  sensor_id==8 

sigma=  [ SIGM A_RAN GE_P VA2  0;0  range2*SIGMA_BEARING_PVA2]; 
elseif  sensor_id==9 

sigma=[SIGMA_RANGE_MPACA2  0;0  range2 * S IGM A_BE ARfN G_MP AC A2 ] ; 
end 

R=ROT*sigma*ROT’; 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  file  executes  the  filter  algorithm 

globals; 

%  Set  up  filter  parameters 

filter. Q=  [DTA2 * SIGM A_XDOTA2  0  0  0; 

0  SIGMA_XDOTA2  0  0; 

0  0  DTA2*SIGMA_YDOTA2  0; 

0  0  0  SIGMA_YDOTA2] ; 

[nsensors,ntime]=size(observations); 

%  Use  sensors 
for  i=l:nsensors 
filter  ,use_sensors(i)=  1 ; 
end 

%  Run  filter 

tracks=tracker(observations, sensors, platforms, filter, times); 

%  Plot  the  track  trajectory 
posttracks; 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  generates  tracks  from  a  sequence  of  observations  and 
%  assesses  its  threat  to  Jurong  Island  "real-time" 

function  tracks=tracker(observations, sensors, platforms, filter, times) 

globals; 

%  Number  of  sensors  and  number  of  reports 
[nsensors,ntimes]=size(observations); 

%  Search  for  first  target  first  observation 
target  =  1 ; 
for  k  =  1  insensors 
forj  =l:ntimes; 

if  (~isnan(observations(k,j).report(target,2:5))) 
temp(j)=j; 
else 

temp(j)=99999; 

end 

end 

starttime(k)  =  min(temp); 
if  (starttime(k)  <=  starttime(l)) 
startstep  =  starttime(k); 
end 
end 

%  Initialise  the  target 
for  j=l 

tracks(startstep  ,j  )=init_tracks(j ,  ob  servations ,  filter,  startstep) ; 
end 

%  Initialise  variables 
for  n=T:ntimes 
kdata(l,n)  =  0; 
kdata(2,n)  =  0; 
kdata(3,n)  =  0; 
kdata(4,n)  =  0; 
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kdata(5,n)  =  0; 
kdata(6,n)  =  0; 
kdata(7,n)  =  99999; 
kdata(8,n)  =  0; 
kdata(9,n)  =  0; 
kdata(10,n)  =  0; 
kdata(l  l,n)  =  0; 
leftTSS(n)  =  NaN; 
leftTSSntoJIHigh(n)  =  NaN; 
leftTSSntoJEV[oderate(n)  =  NaN; 
leftPL(n)  =  NaN; 
leftPLntoJIHigh(n)  =  NaN; 
leftPLntoJIModerate(n)  =  NaN; 
end 

%  Initialise  variables 
distapart  =  0; 
distuncert  =  0; 
disttoreach  =  0; 
speedtoreach  =  0; 
timetoreach  =  0; 

for  i=(startstep+l):ntimes 
startplot=99999; 

buf=sprintf(’Step:  time=%d’,i-l);  disp(buf); 

%  Do  state  prediction  for  the  target 
for  j=l 

tracks(i,j)=state_pred(tracks(i- 1  ,j),times(i), filter); 
end 

%  Do  data  association,  returning  an  obs*track  array  of  associations 
da_array=data_association(tracks, observations, i); 

%  Finally,  do  update  for  the  target 
for  j=l 

tracks(i,j)=state_update(tracks(i,j), observations, fdter,da_array,i,j); 
end 

%  Check  if  SAW  has  entered  Port  Limits 

if  ~isnan(tracks(i,l  ).statc_cst(  1 ))  &&  ~isnan(tracks(i,l).state_est(3)) 

%  Project  the  position  uncertainty  of  the  target  at  final  destination 
FF  =  [1  (ntimes-i)*DT  0  0; 

0  1  0  0; 

0  0  1  (ntimes-i)*DT; 
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00 


0  1]; 

QQ  =  [((ntimes-i)*DT)A2*SIGMA_XDOTA2  0  0  0; 

0  SIGMA_XDOTA2  0  0; 

0  0  ((ntimes-i)*DT)A2*SIGMA_YDOTA2  0; 

0  0  0  SIGMA_YDOTA2] ; 

projstate_pred  =  FF*tracks(i,l).state_est; 
projuncer_pred  =  FF*tracks(i,l).uncer_est*FF'  +  QQ; 
projxuncert  =  sqrt(projuncer_pred(l,l)); 
projyuncert  =  sqrt(projuncer_pred(3,3)); 
projdistuncert  =  sqrt(projxuncertA2  +  projyuncertA2); 

%  Calculate  the  position  uncertainty  of  the  target  at  present  location 
xuncert  =  sqrt(tracks(i,l).uncer_est(l,l)); 

yuncert  =  sqrt(tracks(i,l).uncer_est(3,3)); 

distuncert  =  sqrt(xuncertA2  +  yuncertA2); 

b  =  circle([tracks(i,l).state_est(l),  tracks(i,l).state_est(3)], distuncert); 

if  min(inpolygon  (b.X,  b.Y,  TSS_X,  TSS_Y))==  0  &&... 
min(inpolygon  (b.X,  b.Y,  PL_X,  PL  Y))  ==  0 

buf=sprintf(’Warning:  Entered  Port  Limits  at  Timestep=%d’,i-1);  disp(buf); 

%  Check  if  SAW  is  turning  towards  Jurong  Island 
disttoreach=sqrt(abs(tracks(i,  1 )  .state_est(  1  )-3 02 . 1 1 44444)A2+. . . 

abs(tracks(i,  1 ).  state_est(3 )-347 . 77 1 6667)A2); 
speedtoreach=sqrt(tracks(i,l).state_est(2)A2+... 

tracks(i,  1 )  ,state_est(4)A2); 
timetoreach=disttoreach/speedtoreach; 

%  Calculate  the  time  target  left  Port  Limits 
leftPL(i)  =  MAXTIME-timetoreach; 

%  Calculate  the  distance  between  the  projected  target  end  point 
%  and  Jurong  Island 

distapart  =  sqrt((projstate_pred(l)  -  302.1 144444)A2  +... 

(projstate_pred(3)  -  347.7716667)A2); 

%  Threat  level  assessment 

%  Send  warning  if  target  is  within  0.5nm  to  Jurong  Island 
if  distapart  <  5  &&  MAX  TIME  -  (i-l)*DT  >=  0 
buf=sprintf('Waming:  Heading  Towards  Jurong  Island.  Time  to  Impact  =  %.2f 
sec’,timetoreach);  disp(buf); 

if  projdistuncert  >  distapart 

buf=sprintf(' Warning:  High  Confidence  of  Impact' );disp(buf); 
startplot  =  i-1; 

leftPLntoJIHigh(i)  =  MAXTIME-timetoreach; 
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else 

buf=sprintf(’ Warning:  Moderate  Confidence  of  Impact’ );disp(buf); 
startplot  =  i-1; 

leftPLntoJIModerate(i)  =  MAX_TIME -timetoreach; 
end 
end 

%  Check  if  SAW  has  left  Traffic  Separation  Scheme 
elseif  min(inpolygon  (b.X,  b.Y,  TSS_X,  TSS_Y))==  0  &&... 
min(inpolygon  (b.X,  b.Y,  PL_X,  PL_Y))  ==  1 
buf=sprintf('Warning:  Left  Traffic  Separation  Scheme  at  Timestep=%d',i-1); 
disp(buf); 

%  Check  if  SAW  is  turning  towards  Jurong  Island 
disttoreach=sqrt(abs(tracks(i,  1 )  .state_est(  1  )-3 02 . 1 1 44444)  A2+. . . 

abs(tracks(i,l).state_est(3)-347.7716667)A2); 
speedtoreach  =  sqrt(tracks(i,l).state_est(2)A2+... 

tracks(i,  l).state_est(4)A2); 
timetoreach  =  disttoreach/speedtoreach; 

%  Calculate  the  time  target  left  TSS 
leftTSS(i)  =  MAXTIME-timetoreach; 

%  Calculate  the  distance  between  the  projected  target 
%  end  pointand  Jurong  Island 

distapart  =  sqrt((projstate_pred(l)  -  302.1 144444)A2+... 

(projstate_pred(3)  -  347.77 1 6667)A2); 

%  Threat  level  assessment 

%  Send  warning  if  target  is  within  0.5nm  to  Jurong  Island 
if  distapart  <  5  &&  MAX  TIME  -  (i-l)*DT  >=  0  ° 
buf=sprintf('Warning:  Heading  Towards  Jurong  Island.  Time  to  Impact  =  %.2f 
sec', timetoreach);  disp(buf); 

if  projdistuncert  >  distapart 

buf=sprintf(’ Warning:  High  Confidence  of  Impacf);disp(buf); 
startplot=i-l; 

leftTSSntoJIHigh(i)  =  MAXTIME-timetoreach; 

else 

buf=sprintf(’ Warning:  Moderate  Confidence  of  Impact’ );disp(buf); 
startplot=i-l; 

leftTSSntoJIModerate(i)  =  MAXTIME-timetoreach; 
end 
end 
end 

kdata(l,i)  =  tracks(i,l).time; 
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kdata(2,i)  =  projdistuncert*  1 80; 
kdata(3,i)  =  distapart*180; 
kdata(4,i)  =  disttoreach*180; 
kdata(5,i)  =  speedtoreach*180; 
kdata(6,i)  =  timetoreach; 
kdata(7,i)  =  startplot; 
kdata(8,i)  =  projuncer_pred(l,l); 
kdata(9,i)  =  projuncer_pred(3,3); 
kdata(10,i)  =  projstate_pred(l); 
kdata(l  l,i)  =  projstate_pred(3); 

end 

%  Future  work  for  multi-targets 
%  Search  for  first  observation  for  other  targets 
if  NUMT  ARGETS  ~=1 
for  target=2  :NUM_T  ARGETS 
for  k  =  1  :nsensors 
forj  =l:ntimes; 

if  (~isnan(observations(k,j).report(target,2:5))) 
temp(j)=j; 
else 

temp(j)=99999; 

end 

end 

starttime(k)  =  min(temp); 
if  (starttime(k)  <=  starttime(l)) 
startstep  =  starttime(k); 
end 
end 
end 
end 

%  Initialisation 

for  j=2  :NUM_T  ARGETS 

tracks(startstep,j)=init_tracks(j, observations, filter, startstep); 
end 

%  Track 

for  r=(startstep+l):ntimes 
%  Prediction  for  each  target 
for  j=2  :NUM_T  ARGETS 
tracks(i,j)=state_pred(tracks(i- 1  ,j),times(i), filter); 
end 
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%  Data  association,  returning  an  obs*track  array  of  associations 
da_array=data_association(  tracks, observations, i); 

%  Update  for  each  target 
for  j=2  :NUM_TARGETS 

tracks(i,j)=state_update(tracks(i,j),  observations,  filter, da_array,i,j); 
end 
end 
end 

figure(20); 

elf; 

plot(kdata(l,:),(kdata(2,:)),'b',kdata(l,:),kdata(3,:),'g') 
legend('Distance  Uncertainty',’Distance  Apart'); 
buf=sprintf('Estimated  End  Point'); 
title(buf) 

%set(gca,’XDir', ’reverse’) 

xlim([min(kdata(7, 1 000:MAX_TIME/DT))*DT  MAX  TIME- 1  *DT]) 
axis  ’auto  y’ 

xlabel(’Simulated  Time  (s)’) 
ylabel(’Distance  (m)’) 

figure(21); 

elf; 

plot(kdata(l,:),kdata(4,:),'b’,kdata(l,:),kdata(5,:),'g',kdata(l,:),kdata(6,:),’r’) 
legend('Distance  to  Impact’, ’Speed  to  Impact',  'Time  to  Impact'); 
buf=sprintf('Projected  Distance,  Speed  and  Time  to  Impact’); 
title(buf) 

xlim( [min(kdata(7, 1 000:MAX_TIME/DT))*DT  MAX  TIME- 1  *DT]) 
axis  'auto  y' 

xlabel(’Simulated  Time  (s)’) 

%  Record  the  times  the  target  crossed  TSS,  PL,  &  corresponding  threat  level 
firstleftTSS  =  min(leftTSS(1000:MAX_TIME/DT)); 

TirstleftTSSntoJIHigh  =  min(leftTSSntoJIHigh(1000:MAX_TIME/DT)); 
firstleftTSSntoJIModerate  =  min(leftTSSntoJIModerate(1000:MAX_TIME/DT)); 
firstleftPL  =  min(leftPL(  1 000  :M  AX_TIME/DT)); 
firstleftPLntoJIHigh  =  min(leftPLntoJIHigh(1000:MAX_TIME/DT)); 
firstleftPLntoJIModerate  =  min(leftPLntoJIModerate(1000:MAX_TIME/DT)); 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  initialises  a  set  of  tracks  from  the  first  few  observations 
function  track=init_tracks(ntarget, observations, filter, startstep) 
globals; 

%  Find  the  number  of  sensors 
[nsensors,ntimes]=size(observations); 

%  Now  extract  all  sensor  data  in  the  use  list  associated  with  ntarget 
if  ntarget  ==1 
nz=0; 

for  i=l:nsensors 
if  filter.use_sensors(i)  ==  1 
nz=nz+l; 

%check  for  NaN  first 

if  (~isnan(observations(i,startstep).report(ntarget,2:5))) 
report=observations(i, startstep). report; 
[z(l,nz),z(2,nz),R]=xy_obs(report, ntarget, i); 
end 
end 
end 

%  Specific  for  the  x-y  problem,  now  initialise  the  track 

state_pred=zeros(XSIZE,  1); 

state_pred(l)=738.9629;  %initial  conditions 

state_pred(2)=0 .0303; 

state_pred(3  )=443 . 8223 ; 

state_pred(4)=0 .0303; 

state_est=state_pred; 

uncer_pred=[0. 30864  0  0  0; 

0  0.000408  0  0; 

0  0  0.30864  0; 

0  0  0  0.000408];  %10  knots 

uncer_est=uncer_pred; 
ninnov=nz; 
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innov=zeros(ninnov,  1 ); 
innov_var=zeros(ninnov,ninnov); 

%  put  together  data  structure 

track=make_track(ntarget,  1  ,startstep,state_pred,state_est,. . . 
uncer_pred,uncer_est,innov,innov_var); 
end 

if  ntarget  ~=  1 
nz=0; 

for  i=l  insensors 
if  filter.  use_sensors(i)  ==  1 
nz=nz+l; 

%check  for  NaN  first 

if  (~isnan(observations(i,startstep).report(ntarget,2:5))) 
report=observations(i,startstep). report; 
[z(l,nz),z(2,nz),R]=xy_obs(report, ntarget, i); 
end 
end 
end 

state_pred=zeros(XSIZE,  1); 

state_pred(  1  )=max(z(  1 , 

state_pred(2)=MIN_TARGET_VEL/sqrt(2); 

state_pred(3  )=max(z(2 , : ; 

state_pred(4)=MIN_TARGET_VEL/sqrt(2); 

state_est=state_pred; 

uncer_pred=  1 0  *  filter .  Q ; 
uncer_est=uncer_pred; 

ninnov=nz; 

innov=zeros(ninnov,  1 ); 
innov_var=zeros(ninnov,ninnov); 

%  put  together  data  structure 

track=make_track(ntarget,  1  ,startstep,state_pred,state_est,. . . 
uncer_pred,uncer_est,innov,innov_var); 
end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  This  function  initialises  a  new  track 

function  track=make_track(id, type,  time, state_pred,state_est,... 

uncer_pred,uncer_est,innov,innov_var) 

track=struct('id', id, 'type', type, 'time', time, 'state_pred',state_pred,... 
’state_esf,state_est,'uncer_pred’,uncer_pred,... 
'uncer_esf,uncer_est,’innov',innov,'innov_var',innov_var); 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  performs  prediction  for  a  CVM 

function  track_pred=state_pred(track, time, filter) 

globals; 

dt=time-track.time; 
if  dt  <  0 

error('Negative  prediction  step  in  state_pred'); 
end 

%  Compute  transition  matrix 
F=[l  dt  0  0; 

0  100; 

0  0  1  dt; 

0  0  0  1]; 

%  Do  prediction 
track_pred.time=time; 
state_pred=F  *  track,  state  est; 
uncer_prcd=F*  track.  unccr_cst*F'  +  filter.Q; 

%  Construct  the  new  track 
ninnov=sum(  filter  .usesensors); 
innov=zeros(ninnov,l); 
innov_var=zeros(ninnov,ninnov); 

track_pred=make_track(track.  id,  1  ,time,state_pred,state_pred, . . . 
uncer_pred,uncer_pred,innov,innov_var); 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  sets  up  the  data  association  array 

function  da=data_association(tracks, observations, time) 

%  This  is  an  array  of  size  ntargets  to  nsensors 

%  For  each  sensor  i,  the  entry  states  which  sensor  report 

%  is  associated  with  a  target  j 

[nsensors,  ntimes]=size(observations); 

[ntimes,ntargets]=size(tracks); 

da=zeros(ntargets, nsensors); 

%  For  each  sensor 
for  i=l:nsensors 

%  Associate  a  track  with  a  report 
for  j=l:ntargets 
da(j  ,i)=j ; 
end 
end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 

%  Australian  Centre  for  Field  Robotics 

%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  performs  kalman  filter  update 

function  track=state_update(track, observations, filter, da_array,ntime,ntarget) 

globals; 

[nsensors,temp]=size(observations); 

%  Construct  observation  array 

nz=0; 

for  i=l:nsensors 

if  filter.use_sensors(i)  ==  1 

%  Check  for  valid  observations  first 
if  (~isnan(observations(i,ntime).report(ntarget,2:5))) 
if  (observations(i,ntime).report(ntarget,2:5)  ~=  99999) 

%  Extract  observation 
rep=observations(i,ntime). report; 
na=da_array(ntarget,i); 

[z(l+nz),z(2+nz),Ri]=xy_obs(rep,na,i);  %  Ri  is  unused 
if  i==l 

R(l+nz:2+nz,  l+nz:2+nz)=Rfilter_MPA; 
elseif  i==2 

R(l+nz:2+nz,l+nz:2+nz)=Rfilter_MPA; 
elseif  i==3 

R(l+nz:2+nz,  l+nz:2+nz)=Rfilter_MPA; 
elseif  i=4 

R(l+nz:2+nz,l+nz:2+nz)=Rfilter_MPA; 
elseif  i==5 

R(l+nz:2+nz,  l+nz:2+nz)=Rfilter_MPA; 
elseif  i==6 

R(l+nz:2+nz,l+nz:2+nz)=Rfilter_PCG; 
elseif  i==7 

R(l+nz:2+nz,  l+nz:2+nz)=Rfilter_PCG; 
elseif  i==8 

R(  1  +nz  :2+nz,  1  +nz  :2+nz)=Rfilter_P  V ; 
elseif  i==9 

R(l+nz:2+nz,  l+nz:2+nz)=Rfilter_MPAC; 


158 


end 

H(l+nz,:)=[l  0  0  0]; 

H(2+nz,:)=[0  0  10]; 
nz=nz+2; 

elseif  (observations(i,ntime).report(ntarget,2:5)  ==  99999) 
z(l+nz)=  track.  state_pred(l); 
z(2+nz)=  track.  state_pred(3); 

H(l+nz,:)=[l  0  0  0]; 

H(2+nz,:)=[0  0  10]; 
nz=nz+2; 
end 
end 
end 
end 

%  Do  update 

innov=z'-H*track.state_pred; 
if  (innov  ~=  0) 

S=H*track.uncer_pred*H'+  R; 

W=track.uncer_pred*H'*inv(S); 

track.innov=innov; 

track.  innov_var=S ; 

track.state_est=track.state_pred  +  W*innov; 
track.uncer_est=track.uncer_pred  -  W*S*W'; 
else  %  Propagate  prediction  if  there  is  no  sensor  observation 
track.  state_est=track.  state_pred; 
track.uncer_est=track.uncer_pred; 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  file  displays  the  fused  target  track  following  a  simulation  run 
globals; 

%  Set  plotting  size 

[nsamps,ntargets]=size(tracks); 

figure(l) 

xlim([150  750]) 

ylim(  [280  440]) 

hold  on 

data=zeros(2,nsamps); 

j  =i; 

while  (isempty(tracks(j, l).state_pred(:))) 

j  =  j+i; 

end 

trackstarttime  =  j ; 

for  n=l:ntargets 
for  i=trackstarttime:nsamps 
data(l  ,i)=tracks(i,n).state_est(l); 
data(2,i)=tracks(i,n).state_est(3); 
end 

estp=plot(data(l,trackstarttime:nsamps),data(2,trackstarttime:nsamps),’g-'); 

end 

legend([estp], ’Estimated  Track  Position') 
hold  off 

plot_errors(tracks,  ntargets,  2,  trackstarttime,  targets) 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  produces  various  plots  with  regards  to  the  SAW  parameters 
function  plot_errors(tracks,ntarget,nfigure, trackstarttime, targets) 
globals; 

%  Initialise 

[nsamps,ntargets]=size(tracks); 
zdata=zeros(  1 0,nsamps); 
idata=zeros(  1 0,nsamps); 
pdata=zeros(10,nsamps);  %predicted  data 
edata=zeros(10,nsamps);  %estimated  data 
tdata=zeros(10,nsamps);  %true  data 
esttimesigma=zeros(  1  ,nsamps); 
xmin  =  4500; 
xmax  =  MAXTIME; 

warning  off; 

%  Calculating  and  storing  the  data  for  each  time-step 
for  i=trackstarttime:nsamps 

zdata(  1  ,i)=tracks(i,ntarget).time; 

zdata(2,i)=(tracks(i,ntarget).state_est(  l)-targets(i,ntarget).x)*  1 80; 

zdata(3,i)=(tracks(i,ntarget).state_est(3)-targets(i,ntarget).y)*180; 

zsigma=real(sqrt(tracks(i,ntarget).uncer_est)); 

zdata(4,i)=zsigma(  1 , 1)*  1 80; 

zdata(5  ,i)=zsigma(3 , 3  )  *  1 8  0 ; 

zdata(6,i)=(real(sqrt. . . 

(tracks(i,ntarget) .  state_est(2) . . . 

*tracks(i,ntarget).state_est(2)... 

+tracks(i,ntarget).state_est(4). . . 

*tracks(i,ntarget).state_est(4)))... 

-  targets(i,ntarget).vel)*180; 
zdata(7,i)=atan2(tracks(i,ntarget).state_est(4),... 

tracks(i,ntarget).state_est(2))-targets(i,ntarget).phi; 
if  abs(zdata(7,i))  >  pi 
zdata(7,i)  =  2*pi-abs(zdata(7,i)); 
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end 

zdata(8,i)=sqrt(zsigma(  1 ,  l)*zsigma(  1 ,  l)+zsigma(3,3)*zsigma(3,3))*  1 80; 

zdata(9,i)=sqrt(zsigma(2,2)*zsigma(2,2)+zsigma(4,4)*zsigma(4,4))*180; 

zdata(10,i)=real(... 

sqrt(... 

((tracks(i,ntarget).state_est(2). . . 

/(tracks(i,ntarget).state_est(2)A2  +  ... 
tracks(i,ntarget).state_est(4)A2))A2)... 

*  (zsigma(4 ,4)  A2) . . . 

+  ... 

((tracks(i,ntarget).state_est(4). . . 

/(tracks(i,ntarget).state_est(2)A2  +  ... 
tracks(i,ntarget).state_est(4)A2))A2)... 

*(zsigma(2,2)A2)... 

)... 

); 


tdata(l,i)=tracks(i,ntarget).time; 

tdata(2,i)=targets(i,ntarget).x*  1 80; 

tdata(3,i)=targets(i,ntarget).y*  1 80; 
tdata(6,i)=targets(i,ntarget).vel*  1 80; 
tdata(7 ,  i)=targets(i,ntarget)  .phi ; 

pdata(  1  ,i)=tracks(i,ntarget).time; 
pdata(2,i)=tracks(i,ntarget).state_pred(  1)*  1 80; 
pdata(3,i)=tracks(i,ntarget).state_pred(3)*  1 80; 
psigma=real(sqrt(tracks(i,ntarget).uncer_pred)); 
pdata(4,i)=psigma(  1 , 1)*  1 80; 
pdata(5,i)=psigma(3,3)*  180; 
pdata(6,i)=(real(sqrt... 

(tracks(i,ntarget).state_pred(2)... 

*tracks(i,ntarget).state_pred(2)... 

+tracks(i,ntarget).state_pred(4). . . 

*tracks(i,ntarget).state_pred(4)))... 

)*180; 

pdata(7,i)=atan2(tracks(i,ntarget).state_pred(4),. . . 
tracks(i,ntarget).state_pred(2)); 
if  abs(pdata(7,i))  >  pi 
pdata(7,i)  =  2*pi-abs(pdata(7,i)); 
end 

pdata(8,i)=sqrt(psigma(l,l)*psigma(l,l)+psigma(3,3)*psigma(3,3))*180; 

pdata(9,i)=sqrt(psigma(2,2)!|spsignia(2,2)+psignia(4,4)*psignia(4,4))*180; 

pdata(10,i)=real(... 

sqrt(... 

((tracks(i,ntarget).state_pred(2). . . 
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/(tracks(i,ntarget).state_pred(2)A2  +  ... 

tracks(i,ntarget).state_pred(4)A2))A2)... 

*(psigma(4,4)A2)... 

+  ... 

((tracks(i,ntarget).state_pred(4). . . 
/(tracks(i,ntarget).state_pred(2)A2  +  ... 
tracks(i,ntarget).state_pred(4)A2))A2)... 
*(psigma(2,2)A2)... 

)••• 

); 


edata(  1  ,i)=tracks(i,ntarget).time; 

edata(2 ,  i)=tracks(i,ntarget) .  state_est(  1 )  *  1 8  0 ; 

edata(3,i)=tracks(i,ntarget).state_est(3)*  1 80; 

esigma=real(sqrt(tracks(i,ntarget).uncer_est)); 

edata(4,i)=esigma(  1 , 1)*  1 80; 

edata(5 ,  i)=esigma(3 , 3  ) 1 *  1 8  0 ; 

edata(6,i)=(real(sqrt... 

(tracks(i,ntarget) .  state_est(2) . . . 

*tracks(i,ntarget).state_est(2)... 

+tracks(i,ntarget).state_est(4). . . 

*  tracks(i,ntarget) .  state_est(4))) . . . 

)*180; 

edata(7 ,  i)=atan2  (tracks(i,ntarget) .  state_est(4) , . . . 
tracks(i  ,nt  arget) .  state_est(2)) ; 
if  abs(edata(7,i))  >  pi 
edata(7,i)  =  2*pi-abs(edata(7,i)); 
end 

edata(8,i)=sqrt(esigma(  1 ,  l)*esigma(  1 J)+esigma(3,3)*esigma(3,3))*  1 80; 

edata(9,i)=sqrt(esigma(2,2)*esigma(2,2)+esigma(4,4)*esigma(4,4))*180; 

edata(10,i)=real(... 

sqrt(... 

((tracks(i,ntarget).state_est(2)... 

/(tracks(i,ntarget).state_est(2)A2  +  ... 
tracks(i,ntarget).state_est(4)A2))A2)... 

*(esigma(4,4)A2)... 

+  ... 

((tracks(i,ntarget).state_est(4)... 

/(tracks(i,ntarget).state_est(2)A2  +  ... 
tracks(i,ntarget).state_est(4)A2))A2)... 

*(esigma(2,2)A2)... 

)... 

); 


esttimesigma(  1  ,i)=sqrt(edata(8,i)A2+... 
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(kdata(6,i)A2)*(edata(9,i)A2))/... 

edata(6,i); 

end 

%  merging  the  heading  standard  deviations  for  plotting 
p=tracks  tarttime ;  q=trackstarttime;  i=tracks  tarttime ; 
for  j= 1  :(nsamps-trackstarttime+ 1  )*2 
if  j/2-round(j/2)==0 

combtimedata(  1  j)=  tracks(i,ntarget).time; 

combdata(  8  ,j  )=edata(  8  ,p) ; 

combdata(9,j)=edata(9,p); 

combdata(  1 0,j)=edata(  1 0,p); 

p=p+l; 

i=i+l; 

else 

combtimedata(  1  ,j)=  tracks(i,ntarget).time; 
combdata(8,j)=pdata(8,q); 
combdata(9,j)=pdata(9,q); 
combdata(  1 0,j)=pdata(  1 0,q); 
q=q+l; 
end 
end 

%  Plot 

figure(nfigure); 

elf; 

plot(zdata(l,:),abs(zdata(2,:)),’b',zdata(l,:),zdata(4,:),'r') 
legend('Estimated  Error', 'Standard  Deviation’); 
buf=sprintf('Error  between  X-component  Estimate  and  Truth'); 
title(buf) 

xlim([xmin  xmax]) 
axis  'auto  y' 

xlabel('Simulated  Time  (s)') 
ylabel('X  error  (m)') 

figure(nfigure+ 1 ); 
elf; 

plot(zdata(l,:),  abs(zdata(3,:)),'b',zdata(l,:),zdata(5,:),T) 
legend('Estimated  Error', 'Standard  Deviation'); 
buf=sprintf('Error  between  Y-component  Estimate  and  Truth'); 
title(buf) 

xlim([xmin  xmax]) 
axis  'auto  y' 

xlabel('Simulated  Time  (s)') 
ylabel('Y  error  (m)') 
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figure(nfigure+2) ; 
elf; 

plot(zdata(l,:),abs(zdata(6,:)),’b',zdata(l,:),zdata(9,:),'r') 
legend('Estimated  Error', 'Standard  Deviation'); 
buf=sprintf('Error  between  Velocity  Estimate  and  Truth'); 
title(buf) 

xlim([xmin  xmax]) 
axis  'auto  y' 

xlabel('Simulated  Time  (s)') 
ylabel('Velocity  error  (m/s)') 

figure(nfigure+3 ) ; 
elf; 

plot(zdata(l,:),abs(zdata(7,:)),'b',zdata(l,:),zdata(10,:),'r') 
legend('Estimated  Error', 'Standard  Deviation’); 
buf=sprintf('Error  between  Heading  Estimate  and  Truth'); 
title(buf) 

xlim([xmin  xmax]) 
axis  'auto  y' 

xlabel('Simulated  Time  (s)') 
ylabel('Heading  error  (rads)') 

figure(nfigure+4) ; 
elf; 

plot(pdata(l,:),pdata(2,:),'r',edata(l,:),edata(2,:),’g’,  tdata(l,:),tdata(2,:),'b') 
legend('Predicted  X’, 'Estimated  X','True  X'); 
buf=sprintf('X  position'); 
title(buf) 

xlim([xmin  xmax]) 
axis  'auto  y' 

xlabel(’Simulated  Time  (s)') 
ylabel('X  Position  (m)') 

figure(nfigure+5  ) ; 
elf; 

plot(pdata(l,:),pdata(3,:),'r',edata(l,:),edata(3,:),’g’,  tdata(l,:),tdata(3,:),'b') 
legend('Predicted  Y', 'Estimated  Y',’True  Y'); 
buf=sprintf('Y  position'); 
title(buf) 

xlim([xmin  xmax]) 
axis  'auto  y' 

xlabel(’Simulated  Time  (s)') 
ylabel(’Y  Position  (m)') 
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figure(nfigure+6) ; 
elf; 

plot(pdata(l,:),pdata(6,:),'r',edata(l,:),edata(6,:),'g',  tdata(l,:),tdata(6,:),'b') 
legend('Predicted  VelocityVEstimated  Velocity','True  Velocity'); 
buf=sprintf('V  elocity'); 
title(buf) 

xlim([xmin  xmax]) 
axis  'auto  y' 

xlabel(’Simulated  Time  (s)') 
ylabel('Velocity  (m/s)’) 

figure(nfigure+7) ; 
elf; 

plot(pdata(l,:),pdata(7,:),’r',edata(l,:),edata(7,:),'g',  tdata(l,:),tdata(7,:),'b') 
legend('Predicted  Heading'/Estimated  Heading','True  Heading'); 
buf=sprintf('Heading'); 
title(buf) 

xlim([xmin  xmax]) 
axis  'auto  y' 

xlabel('Simulated  Time  (s)') 
ylabel('Heading  (rad)') 

figure(nfigure+8); 

elf; 

plot(pdata(l,:),pdata(8,:),'r+',edata(l,:),edata(8,:),'g*',combtimedata(l,:),combdata(8,:),’b’) 

legend('Predicted','Estimated'); 

buf=sprintf('Radius  Standard  Deviations'); 

title(buf) 

xlim([xmin  xmax]) 
axis  'auto  y' 

xlabel(’Simulated  Time  (s)') 
ylabel(’Radius  (m)') 

figure(nfigure+9) ; 
elf; 

plot(pdata(l,:),pdata(9,:),'r+',edata(l,:),edata(9,:),’g*',combtimedata(l,:),combdata(9,:),'b') 

legend('Predicted','Estimated'); 

buf=sprintf('Velocity  Standard  Deviations'); 

title(buf) 

xlim([xmin  xmax]) 
axis  'auto  y' 

xlabel('Simulated  Time  (s)') 
ylabel('Velocity  (m/s)') 

figure(nfigure+ 1 0); 
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elf; 

plot(pdata(  1  ,:),pdata(  10,:  ),'r+',edata(  1 ,  :),edata(  1 0,:),'g*  ’,combtimedata(  1 ,  :),combdata(  1 0, 

)/b’) 

legend(’Predicted',’Estimated'); 
buf=sprintf('Heading  Standard  Deviations'); 
title(buf) 

xlim([xmin  xmax]) 
axis  'auto  y' 

xlabel(’Simulated  Time  (s)') 
ylabel(’Heading  (rad)’) 

figure(nfigure+l  1); 
elf; 

plot(edata(  1  ,:),abs((MAX_TIME-edata(l  ,:))- 
kdata(6 , : , 'b edata(  1 , : ), esttimesigma(  1 , : 'r'  ) 
legend('Estimated  Error', 'Standard  Deviation’); 
buf=sprintf('Error  between  Time  Estimate  and  Truth’); 
title(buf) 

xlim([min(kdata(7, 1 000 :MAX_TIME/ 1 0))*DT  xmax- 1  *DT]) 

axis  'auto  y' 

xlabel('Simulated  Time  (s)') 
ylabel('Time  (s)') 

warning  on; 
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B.  BLOCK  2.  CENTRALIZED  DATA  FUSION  ARCHITECTURE  USING 
INFORMATION  FILTER 

%  This  file  executes  the  Monte-Carlo  simulation 

%  Number  of  Monte-Carlo  simulation  runs 
totalruns  =  50; 

%  Initialise  variables 
sumfirstleftTSS(l  Totalruns)  =  NaN; 
sumfirstleftTSSntoJIHigh(l  :totalruns)  =  NaN; 
sumfirstleftTSSntoJIModerate(  1  Totalruns)  =  NaN; 
sumfirstleftPL(  1  Totalruns)  =  NaN; 
sumfirstleftPLntoJIHigh(  1  Totalruns)  =  NaN; 
sumfirstleftPLntoJIModerate(l  Totalruns)  =  NaN; 

%  Execute  each  run  by  generating  new  observations  or  using  previous  ones 
%  Produce  the  fused  track  for  each  run 
%  Then  pool  the  tracks  together  for  analysis 
for  run  =1  Totalruns 

buf=sprintf('Run:  =%d',run);  disp(buf); 
examplesim; 

observations  =  datastore9(run). observations;  %  using  previous  observations 
runifilt; 

sumfirstleftTSS(run)  =  firstleftTSS; 
sumfirstleftTSSntoJIHigh(run)  =  firstleftTSSntoJIHigh; 
sumfirstleftTSSntoJIModerate(run)  =  firstleftTSSntoJIModerate; 
sumfirstleftPL(run)  =  firstleftPL; 
sumfirstleftPLntoJIHigh(run)  =  firstleftPLntoJIHigh; 
sumfirstleftPLntoJIModerate(run)  =  firstleftPLntoJIModerate; 
sumkdata(run,:,:)  =  kdata; 
sumesttimesigma(run,l,:)  =  esttimesigma; 
lowest(run)  =  min(sumkdata(run,7,1000:MAX_TIME/10)); 
end 

%  Calculate  Probability  of  Impact 
Cxtemp  =  squeeze(sumkdata(:,8,:)); 

Cytemp  =  squeeze(sumkdata(:,9,:)); 

Xtemp  =  squeeze(sumkdata(:,10,:)); 

Ytemp  =  squeeze(sumkdata(:,l  1,:)); 
probability  =  zeros(  1  ,ntime); 

warning  off; 
for  1  =  1  :ntime 
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Cx  =  diag(Cxtemp(:,l))*(180A2); 

Cy  =  diag(Cxtemp(:,l))*(180A2); 

X  =  (Xtemp(:,l)-  302.1 144444)*  180; 

Y  =  (Ytemp(:,l)-  347.7716667)*  180; 

W  =  ones(totalruns,l); 
sigma_xbar_tgo  =  inv((W')*inv(Cx)*W); 
sigma_ybar_tgo  =  inv((W')*inv(Cy)*W); 
sigma  =  sqrt(sigma_xbar_tgo); 

xbar_tgo  =  sigma_xbar_tgo*(((W')*inv(Cx)*X));%  Calculate  mean  impact  pt 
ybar_tgo  =  sigma_ybar_tgo*(((W')*inv(Cy)*Y));%  Calculate  mean  impact  pt 
r_not  =  sqrt(xbar_tgoA2  +  ybar_tgoA2); 

R  =  300; 

g  =  zeros(l,100); 
h  =  zeros(l,100); 
probtemp  =  0; 

recur  =  1 ; 

g_not  =  exp(-(r_notA2)/(2*sigmaA2));  %  g_not 
h_not  =  1  -  exp(-(RA2)/(2*sigmaA2));  %  h  not 
probtempnot  =  g_not*h_not; 

g(l)  =  (l/l)*((r_notA2)/(2*sigmaA2))*g_not; 

h(l)  =  -(l/factorial(l))*(((RA2)/(2*sigmaA2))A(l))*exp(-(RA2)/(2*sigmaA2))  +  h_not; 
probtemp  =  probtempnot  +  g(l)*h(l); 

while  (recur  <=  100) 

g(recur+l)  =  (l/(recur+l))*((r_notA2)/(2*sigmaA2))*g(recur); 

h(recur+ 1 )  =  (- 1  /  factorial(recur+ 1  ))*  (((RA2)/ (2  *  sigmaA2))A(recur+ 1 ))  *  exp(- 

(RA2)/(2*sigmaA2))  +  h(recur); 

probtemp  =  probtemp  +  g(recur+l)*h(recur+l); 
recur  =  recur  +  1 ; 
end 

probability(l,l)  =  probtemp; 
end 

%  Initialise  variables 
count  1  =  0; 
count2  =  0; 
count3  =  0; 
count4  =  0; 
count5  =  0; 
count6  =  0; 

SsumfirstleftTSS  =  0; 
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Ssum  H  rstlcftTSSntoJ  I H  igh  =  0; 

Ssum  (1  rstl  c  ftT  S  S  n  toJ  I M  odcratc  =  0; 

Ssum  11  rs  tic  ft  PL  =  0; 

Ssumfi  rsUcftPLntoJIHigh  =  0; 

Ssum  nrstlcftPLnto.il  Moderate  =  0; 

%  Take  the  average  of  the  pooled  tracks 
for  m  =  1  :totalruns 

if  ~isnan(sumfirstleftTSS(m)) 
count  1  =  countl+1; 

SsumfirstleftTSS=SsumfirstleftTSS+sumfirstleftTSS(m); 

end 

if  ~isnan(sumfirstleftTSSntoJIHigh(m)) 
count2  =  count2+l; 

SsumfirstleftTSSntoJIHigh=SsumfirstleftTSSntoJIHigh+sumfirstleftTSSntoJIHigh(m); 

end 

if  ~isnan(sumfi rstlcftTSSntoJ  I  Modcratc(  m)) 
count3  =  count3+l; 

Ssumfi  rstlcftTSSntoJ  I  Modcratc=Ssumfi  rstlcftTSSntoJ  I  Modcratc+sumfi  rstlcftTSSntoJ  I M 
oderate(m); 
end 

if  ~isnan(sumfirstleftPL(m)) 
count4  =  count4+l; 

S  sumfirstleftPL=S  sumfirstleftPL+sumfirstleftPL(m) ; 
end 

if  ~isnan(sumfirstleftPLntoJIHigh(m)) 
count5  =  count5+l; 

SsumfirstleftPLntoJIHigh=SsumfirstleftPLntoJIHigh+sumfirstleftPLntoJIHigh(m); 

end 

if  ~isnan(sumfirstleftPLntoJIModerate(m)) 
count6  =  count6+l; 

SsumfirstleftPLntoJIModerate:=SsumtirstleftPLntoJIModerate+sumfirstleftPLntoJIModer 

ate(m); 

end 

end 

m  can  fi  rstl  cftTSS  =  SsumfirstlcftTSS/count  1 ; 
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m  can  (1  rstl  c  ftTS  S  n  to  J I H  i  gh  =  SsumfirstleftTSSntoJIHigh/count2; 

m  can  11  rstl  c  1  ITS  S  n  to  J I M  o  derate  =  S  s  um  ft  rs  1 1  e  ft  T  S  S  n  to  J I M  o  cle  ratc/c  o  un  t3 ; 

meanfirstleftPL  =  SsumfirstleftPL/count4; 

meanfirstleftPLntoJIHigh  =  SsumfirstleftPLntoJIHigh/count5; 

meanfirstleftPLntoJIModerate  =  S  s  um  11  rstl  c  ft  P  Ln  to  J I M  odcratc/  co  un  t6 ; 

meantruetimedata  =  mean(sumkdata(:,l,:)); 

meandistuncert  =  mean(sumkdata(:,2,:)); 

meandistapart  =  mean(sumkdata(:,3,:)); 

meanesttimedata  =  mean(sumkdata(:,6,:)); 

%  Calculate  the  sample  variance  of  the  estimated  time  to  impact 
for  p  =  1  :ntime 
sumtimediff=0; 
timediff=0; 
for  q  =  1  :totalruns 

timediff  =  (sumkdata(q,6,p)-meanesttimedata(p))A2 ; 
sumtimediff=sumtimediff+timediff; 
end 

meansampletimesigma(p)  =  sqrt(sumtimediff  /(totalruns-1)); 
end 

figure(30); 

elf; 

plot(meantruetimedata(:)',meandistuncert(:)Vb',meantruetimedata(:)',meandistapart(:)','g’) 
legend('Distance  Unccrtainty',’Di stance  Apart'); 
buf=sprintf('Estimated  End  Point'); 
title(buf) 

xlim([min(lowest)*DT  MAX  TIME- 1  *DT]) 
ylim([0  1000]) 
xlabel(’Simulated  Time  (s)') 
ylabel(’Distance  (m)') 

figure(31); 

elf; 

plot(meantruetimedata(:)',abs((MAX_TIME-meantruetimedata(:)')- 
meanesttimedata(:)'),'b’,meantruetimedata(:)',meansampletimesigma,'r' ) 
legend('Estimated  Error', 'Standard  Deviation'); 
buf=sprintf('Error  between  Time  Estimate  and  Truth'); 
title(buf) 

xlim([min(lowest)*DT  MAX  TIME- 1  *DT]) 
ylim(  [0  100]) 

xlabel('Simulated  Time  (s)') 
ylabel('Time  (s)') 

figure(32); 
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elf; 

plot(meantruetimedata(:)',probability(:),’k’) 
legend('Probability  of  Impact'); 
buf=sprintf('Probability  of  Impact'); 
title(buf) 

xlim([min(lowest)*DT  MAX  TIME- 1  *DT]) 
ylim([0  1]) 

xlabel(’Simulated  Time  (s)') 
ylabel('Probability  of  Impact’) 
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%  This  file  executes  the  Monte-Carlo  simulation  and  incorporates  dataloss 
%  (i.e.  observations  loss)  due  to  communication  bandwidth  limitations 

%  Number  of  Monte-Carlo  simulation  runs 
totalruns  =  5; 

sumfirstleftTSS(l:totalruns)  =  NaN; 
sumfirstleftTSSntoJIHigh(l  Totalruns)  =  NaN; 
sumfirstleftTSSntoJIModerate(l  Totalruns)  =  NaN; 
sumfirstleftPL(  1  Totalruns)  =  NaN; 
sumfirstleftPLnto  JIHigh(  1  Totalruns)  =  NaN; 
sumfirstleftPLntoJIModerate(l  Totalruns)  =  NaN; 


for  run  =1  Totalruns 

buf=sprintf('Run:  =%d',run);  disp(buf); 
examplesim; 

%observations  =  datastoreloss(run). observations;  %  using  previous  observations 

%  Since  previous  observations  (with  loss)  is  used,  there  is  no  need  to 
%  re-inject  data  loss.  If  new  observations  (with  no  loss)  are  made,  the 
%  following  codes  are  necessary  to  inject  data  loss. 

% - 

%  %  simulate  data  loss  due  to  bandwidth  limitations 

%  fori  =1:10 

%  j  =  randi(MAX_TIME/DT- 1 060-3)+ 1 060; 

% 

%  datalossduration  =  randi(3); 

%  fork=  1  .'datalossduration 

%  for  n  =  1  :NUM_PLATFORMS 

%  observations(n,j+k).report(2:5)  =99999; 

%  end 

%  end 

%  end 

%  datastoreloss(run).observations=observations; 

% - 


runifilt; 

sumfirstleftTSS(run)  =  firstleftTSS; 
sumfirstleftTSSntoJIHigh(run)  =  firstleftTSSntoJIHigh; 
sumfirstleftTSSntoJIModerate(run)  =  firstleftTSSntoJIModerate; 
sumfirstleftPL(run)  =  firstleftPL; 
sumfirstleftPLntoJIHigh(run)  =  firstleftPLntoJIHigh; 
sumfirstleftPLntoJIModerate(run)  =  firstleftPLntoJIModerate; 
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sumkdata(run,:,:)  =  kdata; 
sumesttimesigma(run,l,:)  =  esttimesigma; 
lowest(run)  =  min(sumkdata(run,7,1000:MAX_TIME/10)); 
end 

%  Calculate  Probability  of  Impact 
Cxtemp  =  squeeze(sumkdata(:,8,:)); 

Cytemp  =  squeeze(sumkdata(:,9,:)); 

Xtemp  =  squeeze(sumkdata(:,10,:)); 

Ytemp  =  squeeze(sumkdata(:,l  1,:)); 
probability  =  zeros(l,ntime); 

warning  off; 
for  1  =  1  :ntime 

Cx  =  diag(Cxtemp(:,l))*(180A2); 

Cy=  diag(Cxtemp(:,l))*(180A2); 

X  =  (Xtemp(:,l)-  302.1 144444)*  180; 

Y  =  (Ytemp(:,l)-  347.7716667)*  180; 

W  =  ones(totalruns,l); 
sigma_xbar_tgo  =  inv((W')*inv(Cx)*W); 
sigma_ybar_tgo  =  inv((W')*inv(Cy)*W); 
sigma  =  sqrt(sigma_xbar_tgo); 

xbar_tgo  =  sigma_xbar_tgo*(((W')*inv(Cx)*X));%  Calculate  mean  impact  pt 
ybar_tgo  =  sigma_ybar_tgo*(((W')*inv(Cy)*Y));%  Calculate  mean  impact  pt 
r_not  =  sqrt(xbar_tgoA2  +  ybar_tgoA2); 

R  =  300; 

g  =  zeros(l,100); 
h  =  zeros(l,100); 
probtemp  =  0; 

recur  =  1 ; 

g_not  =  exp(-(r_notA2)/(2*sigmaA2));  %  g_not 
h_not  =  1  -  exp(-(RA2)/(2*sigmaA2));  %  h  not 
probtempnot  =  g_not*h_not; 

g(l)  =  (l/l)*((r_notA2)/(2*sigmaA2))*g_not; 

h(l)  =  -(l/factorial(l))*(((RA2)/(2*sigmaA2))A(l))*exp(-(RA2)/(2*sigmaA2))  +  h_not; 
probtemp  =  probtempnot  +  g(l)*h(l); 

while  (recur  <=  100) 

g(recur+l)  =  (l/(recur+l))*((r_notA2)/(2*sigmaA2))*g(recur); 
h(recur+ 1 )  =  (- 1  /  factorial(recur+ 1  ))*  (((RA2)/ (2  *  sigmaA2))A(recur+ 1 ))  *  exp(- 

(RA2)/(2*sigmaA2))  +  h(recur); 
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probtemp  =  probtemp  +  g(rccur+ 1  )*h(rccur+ 1 ); 
recur  =  recur  +  1 ; 
end 

probability(l,l)  =  probtemp; 
end 

%  Initialise  variables 
count  1  =  0; 
count2  =  0; 
count3  =  0; 
count4  =  0; 
count5  =  0; 
count6  =  0; 

SsumfirstleftTSS  =  0; 

SsumlirstlcftTSSnloJIHigh  =  0; 

S  sum  fi  rstl  c  ftT  S  S  n  to.1 1 M  odcratc  =  0; 

Ssumfi  rstlcftPL  =  0; 

Ssumfi  rstleftPLntoJIHigh  =  0; 

Ssumfi  rstlcftPLnto.il  Moderate  =  0; 

%  Take  the  average  of  the  pooled  tracks 
for  m  =  1  :totalruns 

if  ~isnan(sumfirstlcftTSS(m)) 
count  1  =  countl+1; 

SsumfirstleftTSS=SsumfirstleftTSS+sumfirstleftTSS(m); 

end 

if  ~isnan(sumfirstleftTSSntoJIHigh(m)) 
count2  =  count2+l; 

SsumfirstleftTSSntoJIHigh=SsumfirstleftTSSntoJIHigh+sumfirstleftTSSntoJIHigh(m); 

end 

if  ~isnan(sumfirstlcftTSSntoJIModeratc(m)) 
count3  =  count3+l; 

Ssumfi  rstlcftTSSntoJIModcratc=Ssum  (i  rstlcftTSSntoJIModeratc+sum  (i  rstlcftTSSntoJIM 
oderate(m); 
end 

if  ~i  snan(  sum  fi  rstl  c  f'tP  L( m ) ) 
count4  =  count4+l; 

S  sumfirstleftPL=S  sumfirstleftPL+sumfirstleftPL(m) ; 
end 
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if  ~isnan(sumfirstleftPLntoJffligh(m)) 
count5  =  count5+l; 

SsumfirstleftPLntoJIHigh=SsumfirstleftPLntoJIHigh+sumfirstleftPLntoJIHigh(m); 

end 

if  ~isnan(sumfirstleftPLntoJIModerate(m)) 
count6  =  count6+l; 

SsumfirstleftPLntoJIModerate=SsumfirstleftPLntoJIModerate+sumfirstleftPLntoJIModer 

ate(m); 

end 

end 

mean  firstlcfffSS  =  SsumlirstlcftTSS/count  1 ; 

mean  (i  rstl  c  ftTS  S  n  to  J I H  i  gh  =  SsumfirstleftTSSntoJIHigh/count2; 

mean  (i  rstl  c  ftTS  S  n  to  J I M  o  derate  =  SsumfirstleftTSSntoJIModerate/count3 ; 

mcanfirstlcftPL  =  SsumfirstleftPL/count4; 

m  can  (i  rstl  c  ft  P  Ln  to  J I H  i  gh  =  SsumfirstleftPLntoJIHigh/count5; 

meanfirstleftPLntoJIModerate  =  SsumfirstleftPLntoJIModerate/count6; 

meantruetimedata  =  mean(sumkdata(:,l,:)); 

meandistuncert  =  mean(sumkdata(:,2,:)); 

meandistapart  =  mean(sumkdata(:,3,0); 

meanesttimedata  =  mean(sumkdata(:,6,:)); 

%  Calculate  the  sample  variance  of  the  estimated  time  to  impact 
for  p  =  1  :ntime 
sumtimediff=0; 
timediff=0; 
for  q  =  1  :totalruns 

timediff  =  (sumkdata(q,6,p)-meanesttimedata(p))A2; 
sumtimediff=sumtimediff+timediff; 
end 

meansampletimesigma(p)  =  sqrt(sumtimediff  /(totalruns-1)); 
end 

figure(30); 

elf; 

plot(meantruetimedata(:)',meandistuncert(:)7b',meantruetimedata(:)',meandistapart(:)7g’) 

legend('Distance  Uncertainty7Distance  Apart'); 
buf=sprintf(’Estimated  End  Point'); 
title(buf) 

xlim([min(lowest)*DT  MAX  TIME- 1  *DT]) 
ylim(  [0  1000]) 
xlabel('Simulated  Time  (s)') 
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ylabel('Distance  (m)') 


figure(31); 

elf; 

plot(meantruetimedata(:)',abs((MAX_TIME-meantruetimedata(:)')- 
meanesttimedata(:)'),'b',meantruetimedata(:)',meansampletimesigma,'r' ) 
legend('Estimated  Error’,'Standard  Deviation'); 
buf=sprintf('Error  between  Time  Estimate  and  Truth'); 
title(buf) 

xlim([min(lowest)*DT  MAX  TIME- 1  *DT]) 
ylim([0  100]) 

xlabel(’Simulated  Time  (s)') 
ylabel('Time  (s)') 

figure(32); 

elf; 

plot(meantmetimedata(:)', probability])), 'k') 
legend('Probability  of  Impact'); 
buf=sprintf('Probability  of  Impact'); 
title(buf) 

xlim([min(lowest)*DT  MAX  TIME- 1  *DT]) 
ylim([0  1]) 

xlabel(’Simulated  Time  (s)') 
ylabel('Probability  of  Impact') 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  is  a  script  file  allows  for  multi-target  multi-sensor  tracking 

%  Define  and  initialise  global  variables 

globals; 

ginit; 

buf=sprintf('Generating  Target  Information.. Please  Waifin’);  disp(buf); 
%  Set  up  time  base  for  target  simulations 
times=0:DT:MAX_TIME; 

%  Generate  all  target’s  attack  destinations 
destinations=generate_destinations(times); 

%  Generate  all  target  trajectories 
targets=generate_targets(times); 

%  Generate  platform  information 
platforms=generate_platforms(times); 

%  Assign  sensors  to  platforms 
sensors=assign_sensors(platforms); 

buf=sprintf('Generating  Observations. ..Please  Wait\n');  disp(buf); 

%  Generate  observations 

observations=generate_observations(sensors, platforms, targets, times); 
buf=sprintf('Displaying  InformationW);  disp(buf); 

%  Display  track  information 

post_sim(targets, observations, sensors, platforms, destinations); 


%  This  file  defines  the  global  variables 
%  General  information 

global  TSS  X;  %  Traffic  Separation  Scheme  x-coordinates 
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global  TSSY;  %  Traffic  Separation  Scheme  y-coordinates 

global  PL_X;  %  Port  Limits  x-coordinates 

global  PL  Y;  %  Port  Limits  y-coordinates 

global  DT ;  %  simulation  time-step 

global  MAX  TIME;  %  maximum  simulation  time 


%  Target  definitions 

global  TARGETTYPE;  %  defines  type  of  target  (xy  or  V  phi) 

global  NUM  TARGETS;  %  number  of  targets  to  be  simulated 


%  Platfonn  definitions 

global  PLATFORM;  %  Platform  data  [xO,  yO,  phiO,  vel] 
global  NUM  PLATFORMS;  %  number  of  platforms 
global  MAX_RANGE;  %  maximum  observable  range 
global  SIGMA  XDOT; 
global  SIGMA  YDOT; 

%  Destination  definitions 

global  DESTINATION;  %  Destination  data  [xO,  yO,  phiO,  vel] 
global  NUM_DESTINATIONS;%  number  of  destinations 


%  State  and  Sensor  definitions 

global  SIGMA  Q;  %  process  noise  standard  deviation  for  feature  model 

global  SIGMA_RANGE_MPA;  %  Range  observation  noise 

global  SIGMA  BEARING  MPA;  %  Bearing  observation  noise 

global  SIGMA_RANGE_MPAC;  %  Range  observation  noise 

global  SIGMA  BEARING  MPAC;  %  Bearing  observation  noise 

global  SIGMA  RANGE  PV;  %  Range  observation  noise 

global  SIGMA  BEARING  PV;  %  Bearing  observation  noise 

global  SIGMA  RANGE  PCG;  %  Range  observation  noise 

global  SIGMA  BEARING  PCG;  %  Bearing  observation  noise 

global  Rfilter  MPA;  %  observation  noise  covariance  for  MPA 

global  Rfilter  MPAC;  %  observation  noise  covariance  for  MPAC 

global  Rfilter  PV;  %  observation  noise  covariance  for  PV 

global  Rfilter  PCG;  %  observation  noise  covariance  for  PCG 

global  F;  %  state  transition  equation 

global  XSIZE;  %  state  dimension 

global  ZSIZE;  %  observation  dimension 

%  Results  "database" 
global  kdata; 
global  esttimesigma; 
global  startplot; 
global  firstleftTSS; 
global  firstleftTSSntoJIHigh; 
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global  firstleftTSSntoJIModerate; 
global  firstleftPL; 
global  fi  rstl  c  IIP  Ln to J I H  i  gli ; 
global  firstleftPLntoJIModerate; 

%  This  file  initializes  the  global  variables 

%  General  polygon  for  Traffic  Separation  Scheme  (TSS) 

X  =  [738.9629;  700.8405;  503.0354;  343.3085;  301.0754;  188.7756;  142.3798; 
291.7565;  356.6971;  474.1785;  703.9221;  738.9538;  738.9629]; 

Y  =  [443.8223;  412.1040;  377.3690;  303.7337;  330.3691;  359.0929;  316.1193; 
230.0498;  289.3964;  351.7903;  379.3511;  408.0001;  443.8223]; 

TSSX  =  X; 

TSSY  =  Y; 

%  General  polygon  for  Port  Limits,  estimated  0.5  mile  beyond  the  TSS 
[PL_X,  PL_Y]  =  bufferm2('xy',X,Y,5,'ouf); 

MAX_TIME=1 1500;  %  simulation  duration 

DT=10;  %  simulation  time-step 

%  Target  definitions 

NUM_TARGETS=1 ;  %  number  of  targets  to  be  tracked 

%  Platform  definitions 
NUM_PLATFORMS=9; 

%  Destination  definitions 
NUM_DESTINATIONS=l ; 

%  State  and  Sensor  definitions 

SIGMA_RANGE_MPA=5;  %  0.5nm  range  error  equivalent 

SIGMA_BEARING_MPA=0.0525;  %  3  degrees  bearing  error  in  radians 
SIGMA_RANGE_MPAC=6;  %  0.6nm  range  error  equivalent 

SIGMA_BEARING_MPAC=0.0875;  %  5  degrees  bearing  error  in  radians 
SIGMA_RANGE_PV=3;  %  0.3nm  range  error  equivalent 

SIGMA_BEARING_PV=0.0525;  %  3  degrees  bearing  error  in  radians 
SIGMA_RANGE_PCG=4;  %  0.4nm  range  error  equivalent 

SIGMA_BEARING_PCG=0.0875;  %  5  degrees  bearing  error  in  radians 
Rfilter  MPA  =  [ SIGM A  RAN GE  MP AA2  0;0  SIGMA  RANGE  MP AA2] ; 
RfilterMPAC  =  [SIGMA_RANGE_MPACA2  0;0  S IGM  ARAN  GEMP  AC  A2  ] ; 

Rfilter  PV  =  [ S IGM A_RAN GE_P VA2  0;0  SIGMA  RANGE  P VA2] ; 

Rfilter  PCG  =  [SIGMA_RANGE_PCGA2  0;0  SIGMA_RANGE_PCGA2] ; 

XSIZE=4;  %  number  of  state  components 

ZSIZE=2;  %  number  of  sensor  measurement  components 

SIGMA_XDOT=0. 00286;  %  std  dev  of  lknot  in  the  x-direction 
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SIGMA_YDOT=0. 00286; 


%  std  dev  of  lknot  in  the  y-direction 


%  Results  "database" 

kdata  =  [7  MAXTIME/DT+1]; 

function  [latb,lonb]  =  bufferm2(varargin)  %lat, Ion, dist, direction, npts,outputfonnat) 
%BUFFERM2  Computes  buffer  zone  around  a  polygon 

% 

%  [latbjonb]  =  bufferm2(lat, Ion, dist, direction) 

%  [latb,lonb]  =  bufferm2(lat, Ion, dist, direction, npts) 

%  [latb,lonb]  =  bufferm2(lat, Ion, dist, direction, npts, outputformat) 

%  [xb,  yb]  =  bufferm2('xy',x,y, dist, direction, npts, outputformat) 

% 

%  This  function  was  originally  designed  as  a  replacement  for  the  Mapping 
%  Toolbox  function  bufferm,  which  calculates  a  buffer  zone  around  a 
%  polygon.  The  original  bufferm  function  had  some  serious  bugs  that  could 
%  result  in  incorrect  buffer  results  and/or  errors,  and  was  also  very  slow. 

%  As  of  R2006b,  those  bugs  have  been  fixed.  However,  this  version  still 
%  maintains  a  few  advantages  over  the  original: 

% 

%  -  Can  be  applied  to  polygons  in  either  geographical  space  (as  in 
%  bufferm)  or  in  cartesian  coordinates. 

% 

%  -  Better  treatment  of  polygon  holes.  The  original  function  simply 
%  filled  in  all  holes;  this  version  trims  or  pads  holes  according  to  the 
%  buffer  width  given. 

% 

%  Input  and  output  format  is  identical  to  bufferm  unless  the  'xy'  option  is 
%  specified,  so  it  can  be  used  interchangeably. 

% 

%  Input  variables: 

% 

%  lat:  Latitude  values  defining  the  polygon  to  be  buffered. 

%  This  can  be  either  a  NaN-delimited  vector,  or  a  cell 

%  array  containing  individual  polygonal  contours  (each  of 

%  which  is  a  vector).  External  contours  should  be  listed 

%  in  a  clockwise  direction,  and  internal  contours  (holes) 

%  in  a  counterclockwise  direction. 

% 

%  Ion:  Longitude  values  defining  the  polygon  to  be  buffered. 

%  Same  format  as  lat. 

% 

%  dist:  Width  of  buffer,  in  degrees  of  arc  along  the  surface 

%  (unless  'xy'  is  used,  in  which  case  units  correspond  to 

%  x-y  coordinates) 

% 
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%  direction:  ’in'  or  'out' 

% 

%  npts:  Number  of  points  used  to  contruct  the  circles  around 

%  each  polygon  vertex.  If  omitted,  default  is  13. 

% 

%  outputformat:  'vector'  (NaN-delimited  vectors),  'cutvector' 

%  (NaN-clipped  vectors  with  cuts  connecting  holes  to  the 

%  exterior  of  the  polygon),  or  'cell'  (cell  arrays  in 

%  which  each  element  of  the  cell  array  is  a  separate 

%  polygon),  defining  format  of  output.  If  omitted, 

%  default  is  'vector'. 

% 

%  'xy':  If  first  input  is  'xy',  then  data  will  be  assumed  to 

%  lie  on  a  cartesian  plane  rather  than  on  a  sphere.  Use 

%  x  and  y  coordinates  as  first  two  inputs  rather  than  lat 

%  and  Ion.  Units  of  x,  y,  and  distance  should  be  the 

%  same. 

% 

%  Output  variables: 

% 

%  latb:  Latitude  values  for  buffer  polygon 

% 

%  lonb:  Longitude  values  for  buffer  polygon 

% 

%  Example: 

% 

%  load  conus 

%  tol  =  0.1;  %  Tolerance  for  simplifying  polygon  outlines 
%  [reducedlat,  reducedlon]  =  reducem(gtlakelat,  gtlakelon,  tol); 

%  dist  =  1 ;  %  Buffer  distance  in  degrees 
%  [latb,  lonb]  =  bufferm2(reducedlat,  reducedlon,  dist,  'out'); 

%  figure('Renderer', ’painters') 

%  usamap({’MN’,'NY'}) 

%  geoshow(latb,  lonb,  'DisplayType',  'polygon',  'FaceColor',  'yellow') 
%  geoshow(gtlakelat,  gtlakelon,... 

%  'DisplayType',  'polygon',  'FaceColor',  'blue') 

%  geoshow(uslat,  uslon) 

%  geoshow(statelat,  statelon) 

% 

%  See  also: 

% 

%  bufferm,  polybool 
%  Copyright  2010  Kelly  Kearney 
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% - 

%  Check  input 
% - 


error(nargchk(3,7,nargin)); 

%  Detennine  if  geographic  or  cartesian 

if  ischar(varargin{  1 })  &&  strcmp(varargin{  1 },  'xy') 
geo  =  false; 

param  =  varargin(2:end); 
else 

geo  =  true; 
param  =  varargin; 
end 

%  Set  defaults  if  not  provided  as  input 
nparam  =  length(param); 
if  geo 

[lat,  Ion,  dist]  =  deal(param{l:3}); 
else 

[Ion,  lat,  dist]  =  deal(param{  1:3});  %  Ion  =  x,  lat  =  y  for  mental  clarity,  will  switch 
back  at  end 
end 

if  nparam  <  4 
direction  =  ’out’; 
else 

direction  =  param  {4}; 
end 

if  nparam  <  5 
npts  =  13; 
else 

npts  =  param{5}; 
end 

if  nparam  <  6 

outputformat  =  ’vector'; 
else 

outputformat  =  param  {6}; 
end 
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%  Check  format  and  dimensions  of  input 

if  ~ismember(direction,  {’in’,  ’out'}) 

error('Direction  must  be  either  ’’in"  or  "out".'); 
end 

if  ~ismember(outputformat,  {’vector’,  'cutvector',  ’cell'}) 
error('Unrecognized  output  format  flag.’); 
end 

if  ~isnumeric(dist)  ||  numel(dist)  >  1 
error(’Distance  must  be  a  scalar.’) 
end 

if  -isnumeric(npts)  ||  numel(npts)  >  1 

error(’Number  of  points  must  be  a  scalar.’) 
end 

if  iscell(lat) 

for  il  =  1  :numel(lat) 

if  ~isvector(lat{il})  ||  ~isvector(lon{il})  ||  ~isequal(length(lat{il}),  length(lon{il})) 
error(’Lat  (or  x)  and  Ion  (or  y)  must  be  vectors  or  cells  of  vectors  with  identical 
dimensions’); 
end 

lat{il}  =  lat{il}(:); 
lon{il}  =  lon{il}(:); 
end 
else 

if -isvector(lat)  ||  -isvector(lon)  [j  ~iscqual(lcngth(lat),  length(lon)) 

error(’Lat  (or  x)  and  Ion  (or  y)  must  be  vectors  or  cells  of  vectors  with  identical 
dimensions’); 
end 

lat  =  lat(:); 

Ion  =  lon(:); 
end 


%■ 


%  Split  polygon(s)  into 
%  separate  faces 


%■ 


if  iscell(lat) 

[lat,  Ion]  =  polyjoin(lat,  Ion);  %  In  case  multiple  faces  in  one  cell, 
end 


184 


[latcells,  loncells]  =  polysplit(lat,  Ion); 


% - 

%  Create  buffer  shapes 
% - 


plotflag  =  0; 

if  plotflag 

Plt.x  =  Ion; 

Plt.y  =  lat; 

end 

latcrall  =  cell(0); 
loncrall  =  cell(0); 

foripoly=  l:length(latcells) 

%  Circles  around  each  vertex 

if  geo 

[late,  lone]  =  calccircgeo(latcells{ipoly},  loncells  {ipoly},  dist,  npts); 
else 

[lone,  late]  =  calccirccart(loncells{  ipoly},  latcells  {ipoly},  dist,  npts); 
end 

%  Rectangles  around  each  edge 
if  geo 

[latr,  lonr]  =  calcrecgeo(latcells{  ipoly},  loncells  {ipoly},  dist); 
else 

[lonr,  latr]  =  calcreccart(loncells  {ipoly},  latcells  {ipoly},  dist); 
end 

%  Union  of  circles  and  rectangles 

if  plotflag 

Plt.rectx  =  lonr; 

Plt.recty  =  latr; 

Plt.circx  =  lone; 

Plt.circy  =  late; 
end 
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[late,  lone]  =  multipolyunion(latc,  lone); 

[latr,  lonr]  =  multipolyunion(latr,  lonr); 

if  plotflag 

Plt.rectcombox  =  lonr; 

Plt.rectcomboy  =  latr; 

Plt.circcombox  =  lone; 

Plt.circcomboy  =  late; 
end 

[loner,  later]  =  polybool(’+',  lonr,  latr,  lone,  late); 

%  Union  of  new  circle/rectangle  combo  with  that  from  other  faces 

[loncrall,  latcrall]  =  polybool('+’,  loncrall,  latcrall,  loner,  later); 

%  Plotting  (for  debugging  only) 

if  plotflag 

Plt.allx  =  loncrall; 

Pit.  ally  =  latcrall; 

if  ipoly  ==  1 
figure; 

plot(Plt.x,  Plt.y,  ’k’,  ’linewidth',  2); 
hold  on 
end 

plot(cat(2,  Plt.rectx):}),  cat(2,  Plt.recty]:}),  ’b’); 
plot(cat(2,  Plt.circx]:}),  cat(2,  Plt.circy { : }),  ’r'); 
plot(Plt.allx  { 1 } ,  Plt.ally { 1 } ,  'g',  'linewidth',  2); 

end 


% - 

%  Calculate  union/difference 
% - 


switch  direction 
case  'out' 

[lonb,  latb]  =  polybool('+',  loncells,  latcells,  loncrall,  latcrall); 
case  'in' 
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[lonb,  latb]  =  polybool('-’,  loncells,  latcells,  loncrall,  latcrall); 
end 

if  plotflag 

[Plt.yfinal,  Plt.xfinal]  =  polyjoin(latb,  lonb); 

plot(Plt.xfinal,  Plt.yfinal,  'linestyle', ’color’,  [0  .5  0],  ’linewidth’,  2); 
end 


% - 

%  Reformat  output 
% - 


if  ~geo 

y  =  latb;  %  Switch,  since  cartesion  uses  opposite  order 
x  = lonb; 
latb  =  x; 
lonb  =  y; 
end 

switch  outputformat 
case  ’vector' 

[latb,  lonb]  =  polyjoin(latb,  lonb); 
case  ’cutvector’ 

[latb,  lonb]  =  polycut(latb,  lonb); 
case  ’cell’ 
end 


**** 

function  [late,  lone]  =  calccircgeo(lat,  Ion,  radius,  npts) 
%  lat  and  Ion:  n  x  1  vectors 
%  radius:  scalar 

radius  =  ones(length(lat),l)  *  radius; 

[late,  lone]  =  scirclel(lat,  Ion,  radius,  [],[],[],  npts); 
late  =  num2cell(latc,  1); 
lone  =  num2cell(lonc,  1); 

function  [latr,  lonr]  =  calcrecgeo(lat,  Ion,  halfwidth) 

%  lat  and  Ion:  n  x  1  vectors 
%  halfwidth:  scalar 

range  =  halfwidth  *  ones(length(lat)-l,  1); 
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az  =  azimuth(lat(l:end-l),  lon(l:end-l),  lat(2:end),  lon(2:end)); 

[latbl  1  ,lonbl  1  ]  =  reckon(lat(l:end-l),  lon(l:end-l),  range,  az-90); 
[latbrl,lonbrl]  =  reckon(lat(l:end-l),  lon(l:end-l),  range,  az+90); 
[latbl2,lonbl2]  =  reckon(lat(2:end),  lon(2:end),  range,  az-90); 
[latbr2,lonbr2]  =  reckon(lat(2:end),  lon(2:end),  range,  az+90); 

latr  =  [latbl  1  latbl2  latbr2  latbrl  latbl  1  ]'; 
lonr^  [lonbll  lonbl2  lonbr2  lonbrl  lonbll]'; 
latr  =  num2cell(latr,  1); 
lonr  =  num2cell(lonr,  1); 

function  [latu,  lonu]  =  multipolyunion(lat,  Ion) 

%  lat  and  Ion  are  n  x  1  cell  arrays  of  vectors 

latu  =  lat  { 1 } ; 
lonu  =  lon{  1 } ; 

for  ip  =  2:length(lat) 

[lonu,  latu]  =  polybool('+',  lonu,  latu,  Ion  {ip},  lat  {ip}); 
end 

[latu,  lonu]  =  polysplit(latu,  lonu); 


function  [xc,  yc]  =  calccirccart(x,  y,  radius,  npts) 

ang  =  linspace(0,  2*pi,  npts+1); 

ang  =  ang(end-l:-l:l); 

xc  =  bsxfun(@plus,  x,  radius  *  cos(ang)); 

yc  =  bsxfun(@plus,  y,  radius  *  sin(ang)); 

xc  =  num2cell(xc',  1); 

yc  =  num2cell(yc',  1); 

%  if  ~ispolycw(x,y) 

%  [xc,yc]  =  poly2ccw(xc,yc); 

%  end 

function  [xrec,  yrec]  =  calcreccart(x,  y,  halfwidth) 

dx  =  diff(x); 
dy  =  diff(y); 


isl  =  dx  >=  0  &  dy  >=  0; 
is2  =  dx  <  0  &  dy  >=  0; 
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is3  =  dx  <  0  &  dy  <  0; 
is4  =  dx  >=  0  &  dy  <  0; 

ishl  =  dy  ==  0  &  dx  >  0; 
ish2  =  dy  ==  0  &  dx  <  0; 


theta  =  zeros(5,l); 

theta(isl  |  is3)  =  atan(dy(isl  |  is3)./dx(isl  |  is3)); 
theta(is2  |  is4)  =  -atan(dy(is2  |  is4)./dx(is2  |  is4)); 

[xl,xr,yl,yr]  =  deal(zeros(size(dx))); 

xl(isl)  =  -halfwidth  *  sin(theta(isl)); 
xr(isl)  =  halfwidth  *  sin(theta(isl)); 
yl(isl)  =  halfwidth  *  cos(theta(isl)); 
yr(isl)  =  -halfwidth  *  cos(theta(isl)); 

xl(is2)  =  -halfwidth  *  sin(theta(is2)); 
xr(is2)  =  halfwidth  *  sin(theta(is2)); 
yl(is2)  =  -halfwidth  *  cos(theta(is2)); 
yr(is2)  =  halfwidth  *  cos(theta(is2)); 

xl(is3)  =  halfwidth  *  sin(theta(is3)); 
xr(is3)  =  -halfwidth  *  sin(theta(is3)); 
yl(is3)  =  -halfwidth  *  cos(theta(is3)); 
yr(is3)  =  halfwidth  *  cos(theta(is3)); 

xl(is4)  =  halfwidth  *  sin(theta(is4)); 
xr(is4)  =  -halfwidth  *  sin(theta(is4)); 
yl(is4)  =  halfwidth  *  cos(theta(is4)); 
yr(is4)  =  -halfwidth  *  cos(theta(is4)); 

xrec  =  [xl+x(l:end-l)  xl+x(2:end)  xr+x(2:end)  xr+x(l:end-l)  xl+x(l:end-l)]; 
yrec  =  [yl+y(l:end-l)  yl+y(2:end)  yr+y(2:end)  yr+y(l:end-l)  yl+y(l:end-l)]; 

xrec  =  num2cell(xrec,  2); 
yrec  =  num2cell(yrec,  2); 
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%  This  function  draws  a  circle  for  a  given  center  and  radius 
function  bubble  =  circle(center, radius) 

Resolution  =  100; 

THETA=linspace(0,2*pi, Resolution); 
RHO=ones(l,Resolution)*radius; 

[X,Y]  =  pol2cart(THETA,RHO); 
bubble.X=X+center(l); 
bubble.  Y=Y+center(2); 

%  This  function  generates  the  SAW's  intended  destinations 
%  (i.e.  Areas  Al,  A2,  and  A3  of  Jurong  Island) 

function  destinations  =  generate  destinations(times) 

%  Define  the  global  variables 
globals; 

%  Define  the  destination  locations 
for  i=l  :NUM_DESTINATIONS 
%  Area  Al 
if  i==l 

destinations(  1  ,i)  =  makedestination; 
destinations(  1  ,i)  .time=times(  1 ); 
destinations(  1  ,i).id=i; 
destinations(l,i).x  =  302.1 144444; 
destinations(l,i).y  =  347.7716667; 

%  Now  iterate  for  all  times 
[temp  ,ntimes] =size(times) ; 
for  n=2:ntimes 

destinations(n,i)=destination_step(destinations(n- 1  ,i),timcs(n)); 
end 
end 

%  Area  A2 
if  i==2 

destinations(  1  ,i)  =  makedestination; 
destinations(  1  ,i)  .time=times(  1 ); 
destinations(  1  ,i).id=i; 
destinations(l,i).x  =  322.731 1  111; 
destinations(l,i).y  =  367.21 16667; 

%  Now  iterate  for  all  times 
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[temp,ntimes]=size(times); 
for  n=2:ntimes 

destinations(n,i)=destination_step(destinations(n-l,i),times(n)); 

end 

end 

%  Area  A3 
if  i==3 

destinations(  1  ,i)  =  makedestination; 
destinations(  1  ,i)  .time=times(  1 ); 
destinations(  1  ,i).id=i; 

destinations(l,i).x  =  343.3450000; 
destinations(l,i).y  =  382.5572222; 

%  Now  iterate  for  all  times 
[temp,ntimes]=size(times); 
for  n=2:ntimes 

destinations(n,i)=destination_step(destinations(n- 1  ,i),timcs(n)); 
end 
end 
end 

%  This  function  initialises  a  new  destination 
function  new=make_destination 

new=struct('id',0, ’time’, 0,'x',0,’y’,0, ’phi’, 0,'vel’,0, 'gamma', 0); 

%  This  function  is  to  step  a  destination 

function  next_destination=destination_step(destination,time) 

globals; 

%  This  is  a  no  motion  model 
dt=time-destination.time; 

nextdestination  =  make  destination;  %  space  for  destination  data  structure 

nextdestination.  id=destination.  id; 

nextdestination.time  =  time; 

nextdestination.x  =  destination.x; 

nextdestination.y  =  destination.y; 

nextdestination.phi  =  destination.phi; 

nextdestination.vel  =  destination.vel; 

nextdestination.  gamma  =  destination.gamma; 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  generates  true  target  information  for  later  observation 
%  and  tracking.  Dynamics  of  target  are  contained  in  "target  step",  number 
%  targets  is  defined  by  the  globally  defined  variable  NUM_TARGETS.  A  data 
%  point  is  generated  at  each  of  a  set  of  "times.” 

function  targets  =  generate_targets(times) 

%  Define  the  global  variables 
globals; 

%  Define  a  SAW  at  pre-fixed  location  in  the  TSS 
for  i=l 

targets(l,i)  =  make_target; 
targets(  1  ,i).time=times(  1 ); 
targets(l,i).id=i; 
targets(l,i).x  =  738.9616667; 
targets(l,i).y  =  438.7050000; 
targets(  1 ,  i) .  gamma=0 ; 

%  Now  iterate  for  all  times 
[temp,ntimes]=size(times); 
for  n=2:ntimes 

targets(n,i)=target_step_SAW_path(targets(n- 1  ,i),times(n)); 
end 
end 

%  Create  non-SAW  contacts  at  random  locations  within  the  TSS 
%  Not  used  since  this  thesis  covers  a  single  target 
for  i=2:NUM_TARGETS 
targets(l,i)  =  make_target; 
targets(  1  ,i).time=times(  1); 
targets(l,i).id=i; 

x  =  TSS_X(l)*rand; 
y  =  TSS_Y(l)*rand; 

while  (inpolygon  (x,  y,TSS_X,  TSS_Y)  ~=  1) 
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x  =  TSS_X(l)*rand; 
y  =  TSS_Y(l)*rand; 
end 

targets(l,i)-x  =  x; 
targets(l,i).y  =  y; 
targets(l,i)-phi  =  2*pi*rand  -pi; 

targets(l,i).vel  =  MINTARGETVEL  +  (M  AX_T  ARGETVEL- 

MIN_TARGET_VEL)*rand; 
targets(l  ,i)-gamma=0; 

%  Now  iterate  for  all  times 
[temp,ntimes]=size(times); 
for  n=2:ntimes 

targets(n,i)=target_step(targets(n- 1  ,i),timcs(n)); 
end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  This  function  initialises  a  new  target 

function  new=make_target 

new=struct('id',0, 'time', 0,'x',0,'yC0, 'phi', 0,'vel',0, 'gamma', 0); 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  is  the  trajectory  taken  by  the  SAW 

function  next_target=target_step_SAW_path(target,time) 

globals; 

dt=time-target.time; 
nexttarget  =  maketarget; 
next_target.id=target.id; 
nexttarget.time  =  time; 

%  target.vel  =  0.042870370  (in  m/s  which  equates  to  15knots) 

%  target.vel  =  0.059160494  (in  m/s  which  equates  to  20knots) 

%  target.vel  =  0.060018519  (in  m/s  which  equates  to  21knots) 

%  target.vel  =  0.062876543  (in  m/s  which  equates  to  22knots) 

if  (next  target.time  >=  0  &&  next  target.time  <  1086) 
next_target.x  =  target.x  +  dt*0.042870370*cos(-pi+0.720); 
next_target.y  =  target. y  +  dt*0.042870370*sin(-pi+0.720); 
next_target.phi  =  -pi+0.720; 
next_target.vel  =  0.042870370; 

elseif  (next_target.time  >=  1086  &&  next_target.time  <  5890) 
next_target.x  =  target.x  +  dt*0.042870370*cos(-pi+0.170); 
next_target.y  =  target.y  +  dt*0.042870370*sin(-pi+0.170); 
next_target.phi  =  -pi+0.170; 
next_target.vel  =  0.042870370; 

elseif  (next  target.time  >=  5890  &&  next  target.time  <  10199) 
next_target.x  =  target.x  +  dt*0.042870370*cos(-pi+0.430); 
next_target.y  =  target.y  +  dt*0.042870370*sin(-pi+0.430); 
next_target.phi  =  -pi+0.430; 
next_target.vel  =  0.042870370; 

elseif  (next  target.time  >=  10199  &&  next  target.time  <  1 1051) 
next_target.x  =  target.x  +  dt*0.042870370*cos(pi-0.770); 
next_target.y  =  target.y  +  dt*0.042870370*sin(pi-0.770); 
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next_target.phi  =  pi-0.770; 
next_target.vel  =  0.042870370; 

elseif  (next  target.time  >=  11051  &&  nexttarget.time  <  11500) 
nexttarget.x  =  target. x  +  dt*0. 059 160494*cos(pi- 1.360); 
next_target.y  =  target.y  +  dt*0. 059 160494*sin(pi- 1.360); 
nexttarget.phi  =  pi-1.360; 
nexttarget.vel  =  0.059160494; 

elseif  (next_target.time  >=  11500) 
next_target.x  =  target.x; 
next_target.y  =  target.y; 
nexttarget.phi  =  0; 
next_target.vel  =  0; 

end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  generates  trajectories  for  platforms 

function  platforms  =  generate_platforms(times) 

%  Define  the  global  variables 
globals; 

%  Define  5  fixed  platform  initial  locations 

%  Model  the  MPA  radar  stations 
for  i=l  :NUM_PLATFORMS 
if  i==l 

platforms(  l,i)  =  make_platform; 
platforms!  1  ,i).time=times(  1 ); 
platforms!  l,i).id=i; 
platforms!  l,i).x  =  285.4700000; 
platforms!  l,i).y  =  360.8527778; 
platforms! l,i).phi  =  2*pi*rand  -pi; 
platforms!  l,i).vel  =  0; 
platforms!  l,i).gamma=0; 

%  Now  iterate  for  all  times 
[temp,ntimes]=size(times); 
for  n=2:ntimes 

platforms(n,i)=platform_step(platforms(n- 1  ,i),times(n)); 
end 
end 

if  i==2 

platforms!  l,i)  =  make_platform; 
platforms!  1  ,i).time=times(  1 ); 
platforms!  l,i).id=i; 
platforms!  l,i).x  =  342.7972222; 
platforms!  l,i).y  =  311.9850000; 
platforms! l,i).phi  =  2*pi*rand  -pi; 
platforms!  l,i).vel  =  0; 
platforms!  l,i).gamma=0; 
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%  Now  iterate  for  all  times 
[temp,ntimes]=size(times); 
for  n=2:ntimes 

platforms(n,i)=platform_step(platforms(n- 1  ,i),times(n)); 
end 
end 

if  i==3 

platforms(l,i)  =  make_platform; 
platforms(l,i)-time=times(l); 
platforms(l,i)-id=i; 
platforms(l,i).x  =  413.1333333; 
platforms(l,i).y  =  349.8194444; 
platforms(l,i)-phi  =  2*pi*rand  -pi; 

platforms(l,i)-vel  =  0; 

platforms(l,i)-gamma=0; 

%  Now  iterate  for  all  times 
[temp  ,ntimes] =size(times) ; 
for  n=2:ntimes 

platforms(n,i)=platfonn_step(platfonns(n- 1 ,  i),ti  mcs(  n )); 
end 
end 

if  i==4 

platforms(l,i)  =  make_platform; 
platforms(  1  ,i).time=times(  1 ); 
platforms(  1  ,i)-id=i; 
platforms(  1  ,i).x  =  46 1 .476 1 1 1 1 ; 
platforms(  1  ,i).y  =  403 .426 1111; 
platforms(l,i)-phi  =  2*pi*rand  -pi; 
platforms(l,i)-vel  =  0; 
platforms(  1  ,i)-gamma=0; 

%  Now  iterate  for  all  times 
[temp  ,ntimes] =size(times) ; 
for  n=2:ntimes 

platforms(n,i)=platfonn_step(platforms(n- 1  ,i),timcs(n)); 
end 
end 

if  i==5 

platforms(l,i)  =  make_platform; 
platforms(l,i)-time=times(l); 
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platforms(l,i)-id=i; 
platforms(l,i)-x  =  517.0844444; 
platforms(l,i).y  =  421.9305556; 
platforms(l,i)-phi  =  2*pi*rand  -pi; 
platforms(l,i)-vel  =  0; 
platforms(  1  ,i)-gamma=0; 

%  Now  iterate  for  all  times 
[temp,ntimes]=size(times); 
for  n=2:ntimes 

platforms(n,i)=platfomi_step(platforms(n- 1 ,  i),ti  mes(  n )); 
end 
end 

%  Define  3  mobile  platform  initial  locations 

%  Model  the  CPC 
if  i==6 

platforms(l,i)  =  make_platform; 
platforms(  1  ,i).time=times(  1 ); 
platforms(  1  ,i)-id=i; 
platforms(l,i).x  =  474.1785056; 
platforms(l,i).y  =  351.7903000; 

platforms(l,i)-phi  =  2*pi*rand  -pi; 
platforms(l,i)-vel  =  0; 

targets(  1 ,  i) .  gamma=0 ; 

%  Now  iterate  for  all  times 
[temp  ,ntimes] =size(times) ; 
for  n=2:ntimes 

platforms(n,i)=platfonn_step_PCG  l_path(platforms(n- 1  ,i),timcs(n)); 
end 
end 

if  i— 7 

platforms(l,i)  =  make_platform; 
platforms(  1  ,i)-time=times(  1 ); 
platforms(  1  ,i)-id=i; 
platforms(l,i)-x  =  474.1785056; 
platforms(l,i).y  =  351.7903000; 
platforms(l,i)-phi  =  2*pi*rand  -pi; 
platforms(l,i)-vel  =  0; 
targets(  1 ,  i) .  gamma=0 ; 

%  Now  iterate  for  all  times 
[temp,ntimes]=size(times); 
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for  n=2:ntimes 

platforms(n,i)=platfomi_step_PCG2_path(platforais(n-l,i),times(n)); 

end 

end 

%  Model  the  PV 
if  i==8 

platforms(l,i)  =  make_platform; 
platforms(  1  ,i).time=times(  1 ); 
platforms(  1  ,i).id=i; 
platforms!  l,i).x  =  474.1785056; 
platforms!  l,i).y  =  351.7903000; 
platforms! l,i)-phi  =  2*pi*rand  -pi; 
platforms!  l,i)-vel  =  0; 
targets!  1  ,i).gamma=0; 

%  Now  iterate  for  all  times 
[temp,ntimes]=size(times); 
for  n=2:ntimes 

platforms(n,i)=platform_stcp_PV_path!platforms!n-l  ,i),timcs(n)); 
end 
end 

%  Given  the  speed  and  sensor  coverage  of  the  MPaA, 

%  its  surveying  of  the  small  TSS  can  be  modeled  as  a  "fixed"  sensor 
%  with  complete  of  the  TSS  during  its  operation. 

%  Model  the  MPaA 
if  i==9 

platforms!  1  ,i)  =  make_platfonn;  %  make  the  platform  data  structure 

platforms!  1  ,i)-time=times(  1); 

platforms!  1  ,i)-id=i; 

platforms!  l,i).x  =  474.1785056; 

platforms!  l,i).y  =  351.7903000; 

platforms! l,i)  phi  =  2*pi*rand  -pi; 

platforms!  l,i)-vel  =  0; 

targets!  1  ,i)-gamma=0; 

%  Now  iterate  for  all  times 
[temp,ntimes]=size(times); 
for  n=2:ntimes 

platforms(n,i)=platfonn_step_MPAC(platforms(n- 1  ,i),timcs!n)); 
end 
end 
end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  This  function  initialises  a  new  platfonu 

function  new=make_platform 

new=struct('id',0, 'time', 0,'x',0,'y',0, 'phi', 0,'vel’,0, 'gamma', 0); 

%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  This  is  a  no  motion  platform  model  for  MPA  radar  station 

function  next_platform=platform_step(platfonn,time) 

globals; 

dt=timc-plat  form,  time; 
next_platform  =  make_platfonn; 
next_platform.id=platform.id; 
next_platform.time  =  time; 
next_platform.x  =  platfonn.x; 
next_platform.y  =  platform.y; 
next_platform.phi  =  platform.phi; 
next_platform.vel  =  platform,  vel; 
next_platform.gamma  =  platform.gamma; 

%  This  is  the  platform  motion  model  for  one  of  the  two  CPCs  for  a  day 
function  next_platform=platform_step_PCG  l_path(platform,time) 
globals; 

dt=time-platform.time; 
next_platform  =  make_platform; 
next_platform.id=platform.id; 
next_platform.time  =  time; 

if  (next_platform.time  >=  0) 
next_platform.x  =  474.1785056; 
next_platform.y  =  351.7903000; 
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next_platform.phi  =  platform.phi; 
next_platfonn.vel  =  platform.vel; 
next_platfonn.  gamma  =  platform,  gamma; 
end 


% -  -  - 

%  This  models  the  platform's  patrol  path.  However,  by  making  the  assumption 
%  that  the  sensors  have  extended  range  to  simplify  sensor  management,  this 
%  modeling  is  not  used  here.  This  will  be  useful  for  future  work  with  the 
%  inclusion  of  sensor  management. 

% 

%  if  (next_platfonn.time  >=  0  &&  next_platform.time  <  18000) 

%  next_platfonn.x  =  platform.x  +  dt*0.015061617*cos(l.  519574188-0. 5*pi); 
%  next_platfonn.y  =  platform.y  +  dt*0.015061617*sin(l.  519574188-0. 5*pi); 
%  next_platfonn.phi  =  1. 519574188-0. 5*pi; 

%  next_platfonn.vel  =  0.015061617; 

%  next_platfonn. gamma  =  platform,  gamma; 

% 

%  elseif  (next_platform.time  >=  18000  &&  next_platform.time  <  32400) 

%  next_platform.x  =  platform.x  +  dt*0.025102695*cos(l.  519574188+0. 5*pi); 
%  next_platfonn.y  =  platform.y  +  dt*0.025102695*sin(l.  519574188+0. 5*pi); 
%  next_platfonn.phi  =  1. 519574188+0. 5*pi; 

%  next_platform.vel  =  0.025 102695; 

%  next_platform.  gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  32400  &&  next_platform.time  <  46800) 

%  next_platform.x  =  platform.x  +  dt*0.015061617*cos(l.  519574188-0. 5*pi); 
%  next_platform.y  =  platform.y  +  dt*0.015061617*sin(l.  519574188-0. 5*pi); 
%  %next_platform.phi  =  1. 519574188-0. 5*pi; 

%  %next_platform.  vel  =  0.015061617; 

%  %next_platfonn.  gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  46800  &&  next_platform.time  <  61200) 

%  next_platfonn.x  =  platform.x  +  dt*0. 025 102695*cos(l.  5 19574188+0. 5*pi); 
%  next_platfomi.y  =  platform.y  +  dt*0.025102695*sin(l.  519574188+0. 5*pi); 
%  next_platform.phi  =  1. 519574188+0. 5*pi; 

%  next_platfonn.vel  =  0.025 102695; 

%  next_platfonn.  gamma  =  platform,  gamma; 

% 

%  elseif  (next_platform.time  >=  61200  &&  next_platform.time  <  75600) 

%  next_platfonn.x  =  platform.x  +  41*0.015061617*008(1. 5 19574188-0. 5*pi); 
%  next_platfonn.y  =  platform.y  +  dt*0.0 1506 1 6 1 7*sin(  1.5195741 88-0.5*pi); 
%  %next_platform.phi  =  1. 519574188-0. 5*pi; 

%  %next_platform.vel  =  0.015061617; 

%  %next_platform.gamma  =  platform,  gamma; 
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% 

%  elseif  (next_platform.time  >=  75600  &&  next_platform.time  <  86400) 

%  next_platform.x  =  platform.x  +  dt*0. 025 102695*008(1. 5 19574188+0. 5*pi); 
%  next_platform.y  =  platform,  y  +  dt*0.025102695*sin(l.  519574188+0. 5*pi); 
%  next_platform.phi  =  1. 519574188+0. 5*pi; 

%  next_platform.vel  =  0.025 102695; 

%  next_platform.  gamma  =  platform,  gamma; 

% 

%  elseif  (next_platform.time  >=  86400) 

%  next_platform.x  =  platform.x; 

%  next_platform.y  =  platform.y; 

%  next_platform.phi  =  0; 

%  next_platform.vel  =  0; 

%  next_platform.  gamma  =  0; 

%  end 

% - 
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%  This  is  the  platform  motion  model  for  one  of  the  two  CPCs  for  a  day 
function  next_platform=platform_step_PCG2_path(platform,time) 
globals; 

dt=time-platform.time; 
next_platfonn  =  make_platfonn; 
next_platfonn.id=platform.id; 
next_platfonn.time  =  time; 

if  (next_platform.time  >=  0) 
next_platfonn.x  =  474.1785056; 
next_platfonn.y  =  351.7903000; 
next_platfonn.phi  =  platform.phi; 
next_platfonn.vel  =  platform.vel; 
next_platfonn.  gamma  =  platform. gamma; 
end 


% - 

%  This  models  the  platform's  patrol  path.  However,  by  making  the  assumption 
%  that  the  sensors  have  extended  range  to  simplify  sensor  management,  this 
%  modeling  is  not  used  here.  This  will  be  useful  for  future  work  with  the 
%  inclusion  of  sensor  management. 

% 

%  if  (next_platfonn.time  >=  0  &&  next_platform.time  <  18000) 

%  next_platform.x  =  platform.x  +  dt*0.013140537*cos(-l. 330818664+0. 5*pi); 
%  next_platfonn.y  =  platform.y  +  dt*0.013140537*sin(-l. 330818664+0. 5*pi); 
%  next_platfonn.phi  =  - 1.3308 18664+0. 5  *pi; 

%  next_platfonn.vel  =  0.013 140537; 

%  next_platfonn. gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  18000  &&  next_platform.time  <  32400) 

%  next_platform.x  =  platform.x  +  dt*0. 02 1900894*cos(- 1.3308 18664-0. 5  *pi); 
%  next_platfonn.y  =  platform.y  +  dt*0. 02 1900894*sin(- 1.3308 18664-0. 5  *pi); 
%  next_platfonn.phi  =  -1. 330818664-0. 5*pi; 

%  next_platfonn.vel  =  0.021900894; 

%  next_platform.  gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  32400  &&  next_platform.time  <  46800) 

%  next_platfonn.x  =  platform.x  +  dt*0.013140537*cos(-l. 330818664+0. 5*pi); 
%  next_platform.y  =  platform.y  +  dt*0.013140537*sin(-l. 330818664+0. 5  *pi); 
%  next_platform.phi  =  - 1.3308 18664+0. 5  *pi; 

%  next_platfonn.vel  =  0.013 140537; 
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%  next_platform.  gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  46800  &&  next_platform.time  <  61200) 

%  next_platform.x  =  platform.x  +  dt*0. 02 1900894*cos(- 1.3308 18664-0. 5  *pi); 
%  next_platform.y  =  platform.y  +  dt*0. 02 1900894*sin(- 1.3308 18664-0. 5  *pi); 
%  next_platform.phi  =  -1. 330818664-0. 5*pi; 

%  next_platform.vel  =  0.021900894; 

%  next_platform. gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  61200  &&  next_platform.time  <  75600) 

%  next_platform.x  =  platform.x  +  dt*0.013140537*cos(-l. 330818664+0. 5*pi); 
%  next_platform.y  =  platform.y  +  dt*0.013140537*sin(-l. 330818664+0. 5  *pi); 
%  next_platform.phi  =  - 1.3308 18664+0. 5  *pi; 

%  next_platfonn.vel  =  0.013 140537; 

%  next_platform.  gamma  =  platform,  gamma; 

% 

%  elseif  (next_platform.time  >=  75600  &&  next_platform.time  <  86400) 

%  next_platform.x  =  platform.x  +  dt*0.021900894*cos(-l. 330818664-0. 5*pi); 
%  next_platform.y  =  platform.y  +  dt*0. 02 1900894*sin(- 1.3308 18664-0. 5  *pi); 
%  next_platform.phi  =  -1. 330818664-0. 5*pi; 

%  next_platfonn.vel  =  0.02 1900894; 

%  next_platfonn.  gamma  =  platform,  gamma; 

% 

%  elseif  (next_platform.time  >=  86400) 

%  next_platform.x  =  platform.x; 

%  next_platform.y  =  platform.y; 

%  next_platform.phi  =  0; 

%  next_platform.vel  =  0; 

%  next_platform.  gamma  =  0; 

%  end 

% - 
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%  This  is  the  platform  motion  model  for  PV  for  a  day 
function  next_platform=platform_step_PV_path(platform,time) 
globals; 

dt=time-platform.time; 
next_platfonn  =  make_platfonn; 
next_platfonn.id=platform.id; 
next_platfonn.time  =  time; 

if  (next_platform.time  >=  0) 
next_platfonn.x  =  474.1785056; 
next_platfonn.y  =  351.7903000; 
next_platfonn.phi  =  platform.phi; 
next_platfonn.vel  =  platform.vel; 
next_platfonn.  gamma  =  platform. gamma; 
end 


% - 

%  This  models  the  platform's  patrol  path.  However,  by  making  the  assumption 
%  that  the  sensors  have  extended  range  to  simplify  sensor  management,  this 
%  modeling  is  not  used  here.  This  will  be  useful  for  future  work  with  the 
%  inclusion  of  sensor  management. 

% 

%  if  (next_platfonn.time  >=  0  &&  next_platform.time  <  43200) 

%  next_platform.x  =  platform.x  +  dt*0.013972407*cos(0. 1528 13367); 

%  next_platfonn.y  =  platform.y  +  dt*0.013972407*sin(0. 1528 13367); 

%  next_platfonn.phi  =  0.152813367; 

%  next_platfonn.vel  =  0.013972407; 

%  next_platfonn. gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  43200  &&  next_platform.time  <  86400) 

%  next_platform.x  =  platform.x  +  dt*0.013972407*cos(0.152813367+pi); 

%  next_platform.y  =  platform.y  +  dt*0.013972407*sin(0.152813367+pi); 

%  next_platfonn.phi  =  0.152813367+pi; 

%  next_platfonn.vel  =  0.013972407; 

%  next_platform.  gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  86400) 

%  next_platfonn.x  =  platform.x; 

%  next_platform.y  =  platform.y; 

%  next_platform.phi  =  0; 

%  next_platfonn.vel  =  0; 
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%  next_platform. gamma  =  0; 

%  end 

% - - - 


207 


%  This  is  the  platform  motion  model  for  MPaA 

function  next_platfonn=platform_step_MPAC(platfonn,time) 

globals; 

next_platfonn  =  make_platfonn; 
next_platfonn.id=platform.id; 
next_platfonn.time  =  time; 

if  (next_platform.time  >=  0) 
next_platfonn.x  =  474.1785056; 
next_platfonn.y  =  351.7903000; 
next_platfonn.phi  =  platform.phi; 
next_platfonn.vel  =  platform.vel; 
next_platfonn.  gamma  =  platform. gamma; 
end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  creates  and  assigns  sensors  to  platforms 
%  It  assigns  one  sensor  to  one  platform 

function  sensors=assign_sensors(platforms) 

globals; 

%  Define  sensors  for  the  5  fixed  platforms 

for  i=l  :NUM_PLATFORMS 
if  i==l 

sensors(i)=make_sensor; 

sensors(i).id=i; 

sensors(i).platform=i; 

sensors(i).r_err=SIGMA_RANGE_MPA; 

sensors(i).b_err=SIGMA_BEARING_MPA; 

sensors(i).point=0;  %  zero  point  angle 

sensors(i).beam_view=2*pi; 

sensors(i).max_range=154;  %  an  equivalent  of  15nm 


elseif  i==2 

sensors(i)=make_sensor; 

sensors(i).id=i; 

sensors(i).platform=i; 

sensors(i).r_err=SIGMA_RANGE_MPA; 

sensors(i).b_err=SIGMA_BEARING_MPA; 

sensors(i)  .point=0 ; 

scnsors(i).beam_vicw=2*pi; 

sensors(i).max_range=l  54; 


elseif  i==3 

sensors(i)=make_sensor; 

sensors(i).id=i; 

sensors(i).platform=i; 

sensors(i).r_err=SIGMA_RANGE_MPA; 

sensors(i).b_err=SIGMA_BEARING_MPA; 

sensors(i).point=0; 
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sensors(i).beam_view=2*pi; 
sensors(i).max_range=l  54; 


elseif  i==4 

sensors(i)=make_sensor; 

sensors(i).id=i; 

sensors(i).platform=i; 

sensors(i).r_err=SIGMA_RANGE_MPA; 

sensors(i) .  b_err=S  IGM  A_BE  ARIN  G_MP  A ; 

sensors(i).point=0; 

sensors(i).beam_view=2*pi; 

sensors(i).max_range=l  54; 


elseif  i==5 

sensors(i)=make_sensor; 

sensors(i).id=i; 

sensors(i).platform=i; 

sensors(i) .  r_err=S  IGM  ARAN  GE_MP  A ; 

sensors(i).b_err=SIGMA_BEARING_MPA; 

sensors(i).point=0; 

sensors(i).beam_view=2*pi; 

sensors(i).max_range=l  54; 

%  Define  sensors  for  the  4  mobile  platforms 

%  Define  the  CPC  sensor 
elseif  i==6 

sensors(i)=make_sensor; 

sensors(i).id=i; 

sensors(i).platform=i; 

sensors(i).r_err=SIGMA_RANGE_PCG; 

sensors(i).b_err=SIGMA_BEARING_PCG; 

sensors(i).point=0; 

sensors(i).beam_view=2*pi; 

sensors(i).max_range=1000;  %  range  extended  to  simulate 
%  presence  at  Jurong  Island 

%  Define  the  CPC  sensor 
elseif  i==7 

sensors(i)=make_sensor; 

sensors(i).id=i; 

sensors(i).platform=i; 

sensors(i).r_err=SIGMA_RANGE_PCG; 

sensors(i).b_err=SIGMA_BEARING_PCG; 

sensors(i).point=0; 
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sensors(i).beam_view=2*pi; 

sensors(i).max_range=1000; 

%  Define  the  PV  sensor 
elseif  i==8 

sensors(i)=make_sensor; 

sensors(i).id=i; 

sensors(i).platform=i; 

sensors(i) .  r_err=S  IGM  A_RAN  GE_P V ; 

sensors(i).b_err=SIGMA_BEARING_PV; 

sensors(i).point=0; 

scnsors(i).beam_vicw=2*pi; 

sensors(i).max_range=1000; 

%  Define  the  MPaA  sensor 
elseif  i==9 

sensors(i)=make_sensor; 

sensors(i).id=i; 

sensors(i).platform=i; 

sensors(i) .  r_err=S  IGM  ARAN  GE_MP  AC ; 
sensors(i).b_err=SlGMA_BEARlNG_MPAC; 
sensors(i).point=0; 
sensors(i).beam_view=2*pi; 
sensors(i).max_range=1000; 
end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  This  function  initialises  a  new  sensor 

function  new=make_sensor 

new=struct('id’,0, 'platform', 0,’r_err’,0,’b_err',0, ’point’, 0,... 
’beam_view’,0,’max_range’,0); 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  generates  observations  from  sensors  located  on 
%  platfonns  observing  targets  for  the  stipulated  time-steps 

function  observations=generate_observations(sensors, platforms, tracks, times) 

[temp,nsensors]=size(sensors); 
for  s=l:nsensors 
ntimes=l; 
for  t=times 

observations^, ntimes).sensor=s; 
observations^, ntimes).time=t; 

observations^, ntimes).report=sensor_report(sensors(s),  platforms,  tracks,  t); 
ntimes=ntimes+ 1 ; 
end 
end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  retrieves  the  location  of  the  platform 

function  [px,py]=get_platfonu_loc(pnum, time, platforms) 

[nsamps,nplats]=size(platforms); 
if  (pnum>  nplats)  |  (pnum  <1) 

error('Incorrect  platform  number  specified  in  get_platform_loc'); 
end 

if  (time<platforms(l,pnum).time)|(time>platforms(nsamps,pnum).time) 
error('Time  out  of  range  in  get_platform_loc'); 
end 

it=l; 

while  platforms(it, pnum). time  <  time 
it=it+l; 
end 

px=platforms(it,pnum).x; 
py=platforms(it,pnum)  .y; 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  retrieves  the  location  of  the  target 

function  [tx,ty]=get_target_loc(tnum, time, tracks) 

[nsamps,ntracks]=size(tracks); 
if  (tnum>  ntracks)  |  (tnum  <1) 
error('Incorrect  track  number  specified  in  get  target  loc’); 
end 

if  (time<tracks(  1 , tnum). time)|(time>tracks(nsamps, tnum). time) 
error(’Time  out  of  range  in  get  target  loc’); 
end 

it=l; 

while  tracks(it, tnum). time  <  time 
it=it+l; 
end 

tx=tracks(it,tnum).x; 

ty=tracks(it,tnum).y; 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  constructs  a  report  for  a  sensor  at  a  particular  time 
function  report=sensor_report(sensor, platforms, tracks, time) 
globals; 

report=zeros(NUM_TARGETS,5); 

%  Find  platform  location  at  this  time 
[px,py]=get_platform_loc(sensor  .platform, time, platforms); 

for  tnum=l  :NUM_TARGETS 
%  Find  target  location 
[tx,ty]=get_target_loc(tnum, time, tracks); 

%  Compute  range  and  bearing 

dx=tx-px; 

dy=ty-py; 

range=sqrt(dx*dx  +  dy*dy); 
bearing=atan2(dy,dx); 

%  Determine  if  this  is  actually  visable 
if  range  <  sensor.max  range 
%  Add  error  from  sensor  model 
report(tnum,  1  )=tnum; 

report(tnum,2)=range  +  scnsor.r_crr*(- 1  +25|5rand); 
report(tnum,3)=bearing  +  sensor.b_err*(-l+25|!rand); 
report(tnum,4)=px; 
report(tnum,5)=py; 
else 

report(tnum,  1  )=tnum; 
report(tnum,2)=NaN; 
report(tnum,3)=NaN; 
report(tnum,4)=NaN ; 
report(tnum,  5  )=N  aN ; 
end 
end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  displays  true  target  trajectories,  platfonn  trajectories, 

%  target  distinations,  and  sensor  observations  following  a  simulation  run 

function 

[true,  plat,  obs,dest]=post_sim(targets,  observations,  sensors,  platforms,  destinations) 
globals; 

%  Set  plotting  size 

[nsamps,nplatforms]=size(platforms); 

[nsamps,ndestinations]=size(destinations); 

[nsamps,ntargets]=size(targets); 

[nsensors,nsamps]=size(observations); 

figure(l) 

elf; 

hold  on 

data=zeros(2,nsamps); 
title('True  Target  Motions') 
xlabel(’x-position  (m)’) 
ylabel(’y-position  (m)') 

%  Plot  true  target  trajectories 
for  n=l:ntargets 
for  i=l:nsamps 
data(  1  ,i)=targets(i,n).x; 
data(2,i)=targets(i,n).y; 
end 

true=plot(data(l,:),data(2,:)); 

end 

%  Plot  true  platform  trajectories 
for  n=l  mplatforms 
for  i=l:nsamps 
data(  1  ,i)=platforms(i,n).x; 
data(2,i)=platforms(i,n).y; 
end 
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if  n  ==  1 

plat=plot(data(  1 , : ),  data(2 , : ) ,  ’ko ') ; 
elseif  n  ==  2 

plat=plot(data(l,:),data(2,:),'ko'); 
elseif  n  ==  3 

plat=plot(data(l,:),data(2,:),'ko'); 
elseif  n  ==  4 

plat=plot(data(l,:),data(2,:),’ko’); 
elseif  n  ==  5 

plat=plot(data(l,:),data(2,:),'ko’); 
elseif  n  ==  6 

plat=plot(data(l,:),data(2,:),’r*'); 
elseif  n  ==  7 

plat=plot(data(l,:),data(2,:),'r*'); 
elseif  n  ==  8 

plat=plot(data(l,:),data(2,:),'gh'); 
elseif  n  ==  9 

plat=plot(data(l,:),data(2,:),’bo’); 

end 

end 

%  Plot  true  destination  locations 
for  n=l  mdestinations 
for  i=l:nsamps 
data(  1  ,i)=destinations(i,n).x; 
data(2,i)=destinations(i,n).y; 
end 

dest=plot(data(  1  ,:),data(2,:),'rh’); 
end 

%  Plot  sensor  observations 
for  n=l:nsensors  %  for  all  sensors 
for  i=l  :nsamps  %  for  all  time 
report=observations(n,i).report; 
for  m=l  :ntargets  %  for  all  targets 
if  report(m,l)  >  0  %  if  they  are  seen 

[zx,zy,R]=xy_obs(report,m,n);  %  convert  observation  to  global  xy  coordinates 
odata(l,m)=zx; 
odata(2,m)=zy; 
end 
end 

if  n  ==  1 

obs=plot(odata(l,:),odata(2,:),’k.'); 
elseif  n  ==  2 

obs=plot(odata(l,:),odata(2,:),’k.'); 
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elseif  n  ==  3 

obs=plot(odata(l,:),odata(2,:),’k.'); 
elseif  n  ==  4 

obs=plot(odata(l,:),odata(2,:),'k.'); 
elseif  n  ==  5 

obs=plot(odata(l,:),odata(2,:),’k.'); 
elseif  n  ==  6 

obs=plot(odata(l,:),odata(2,:),'r.'); 
elseif  n  ==  7 

obs=plot(odata(l,:),odata(2,:),’r.'); 
elseif  n  ==  8 

obs=plot(odata(l,:),odata(2,:),’g.'); 
elseif  n  ==  9 

obs=plot(odata(l,:),odata(2,:),’b.'); 

end 

end 

end 

legend([true, plat, obs,dest], ’Target  True  Position’/Tracking 

Observations’,'Intended  Target  Destination') 
hold  off 


Stations','Target 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  extract  an  xy  observation  from  a  range-bearing  report 

function  [zx,zy,R]=xy_obs(rep,n,sensor_id) 

globals; 

sr=sin(rep(n,3)); 
cr=cos(rep(n,3)); 
zx=rep(n,4)+rep(n,2)  *  cr; 
zy=rep(n,  5  )+rep(n,2)  *  sr; 

ROT=[cr  -sr;  sr  cr]; 
rangc2=rep(n,2)*rep(n,2); 
if  sensor_id==l 

sigma=  [ S IGM A_RAN GE_MP AA2  0;0  range2*SIGMA_BEARING_MPAA2]; 
elseif  sensor_id==2 

sigma=  [ S IGM A_RAN GE_MP AA2  0;0  range2*SIGMA_BEARING_MPAA2]; 
elseif  sensor_id==3 

sigma=  [ S IGM A_RAN GE_MP A A2  0;0  ran gc2 *SIGMA_BEARIN G_MPAA2] ; 
elseif  sensor_id==4 

sigma=  [ S IGM A_RAN GE_MP AA2  0;0  range2*SIGMA_BEARING_MPAA2]; 
elseif  sensor_id==5 

sigma=  [ S IGM A_RAN GE_MP A A2  0;0  ran gc2 *SIGMA_BEARIN G_MPAA2] ; 
elseif  sensor_id==6 

sigma=[SIGMA_RANGE_PCGA2  0;0  range2 * SIGMA_BEARIN G_PCGA2] ; 
elseif  sensor_id==7 

sigma=[SIGMA_RANGE_PCGA2  0;0  range2*SIGMA_BEARING_PCGA2]; 
elseif  sensor_id==8 

sigma=  [ S IGM A_RAN GE_P VA2  0;0  range2* SIGMA_BEARING_PVA2] ; 
elseif  sensor_id==9 

sigma=  [ S IGM A_RAN GE_MP AC A2  0;0  range2 * S IGM A_BE ARIN G_MP AC A2 ] ; 
end 

R=ROT*sigma*ROT’; 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  file  executes  the  filter  algorithm 

globals; 

%  Set  up  filter  parameters 

filter. Q=  [DTA2 * SIGM A_XDOTA2  0  0  0; 

0  SIGMA_XDOTA2  0  0; 

0  0  DTA2*SIGMA_YDOTA2  0; 

0  0  0  SIGMA_YDOTA2] ; 

filter.H=[l  0  0  0;  0  0  1  0]; 
[nsensors,ntime]=size(observations); 

%  Use  sensors 
for  i=l:nsensors 
filter  ,use_sensors(i)=  1 ; 
end 

%  Run  filter 

tracks=itracker(observations, sensors, platforms, filter, times); 

%  Plot  the  track  trajectory 
postitracks; 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  generates  tracks  from  a  sequence  of  observations  and 
%  assesses  its  threat  to  Jurong  Island  "real-time" 

function  tracks=itracker( observations, sensors, platforms, filter, times) 

globals; 

%  Number  of  sensors  and  number  of  reports 
[nsensors,ntimes]=size(observations); 

%  Search  for  first  target  first  observation 
target  =  1 ; 
for  k  =  1  insensors 
forj  =l:ntimes; 

if  (~isnan(observations(k,j).report(target,2:5))) 
temp(j)=j; 
else 

temp(j)=99999; 

end 

end 

starttime(k)  =  min(temp); 
if  (starttime(k)  <=  starttime(l)) 
startstep  =  starttime(k); 
end 
end 

%  Initialise  the  target 
forj  =  1 

tracks(startstep  ,j  )=init_itracks(j ,  observations ,  filter,  startstep) ; 
end 

%  Initialise  variables 
for  n  =  1  mtirnes 
kdata(l,n)  =  0; 
kdata(2,n)  =  0; 
kdata(3,n)  =  0; 
kdata(4,n)  =  0; 
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kdata(5,n)  =  0; 
kdata(6,n)  =  0; 
kdata(7,n)  =  99999; 
kdata(8,n)  =  0; 
kdata(9,n)  =  0; 
kdata(10,n)  =  0; 
kdata(l  l,n)  =  0; 
leftTSS(n)  =  NaN; 
leftTSSntoJIHigh(n)  =  NaN; 
leftTSSntoJEV[oderate(n)  =  NaN; 
leftPL(n)  =  NaN; 
leftPLntoJIHigh(n)  =  NaN; 
leftPLntoJIModerate(n)  =  NaN; 
end 

%  Initialise  variables 
distapart  =  0; 
distuncert  =  0; 
disttoreach  =  0; 
speedtoreach  =  0; 
timetoreach  =  0; 

for  i  =  (startstep+l):ntimes 
startplot  =  99999; 

buf  =  sprintf(’Step:  time=%d’,i-l);  disp(buf); 

%  Do  state  prediction  for  the  target 
for  j=l 

tracks(ij)  =  info_pred(tracks(i-l,j),times(i), filter); 
end 

%  Do  data  association,  returning  an  obs*track  array  of  associations 
daarray  =  data_association(  tracks, observations, i); 

%  Finally,  do  update  for  the  target 
for  j=l 

tracks(ij)  =  info_update(tracks(i,j),  observations,  filter, da_array,i,j); 
end 

warning  off; 

Y  =  tracks(i,l).Yest; 

P  =  inv(Y); 

%  Check  if  SAW  has  entered  Port  Limits 
if  P  ~=  inf 

%  Calculate  the  position  uncertainty  of  the  target  at  present  location 
x  =  P*tracks(i,l).yest; 


223 


ysigma  =  sqrt(P); 

xuncert  =  ysigma(  1,1); 

yuncert  =  ysigma(3,3); 

distuncert  =  sqrt(xuncertA2  +  yuncertA2); 

b  =  circle) [x(l),  x(3)], distuncert); 

%  Project  the  position  uncertainty  of  the  target 
%  at  final  destination 
FF  =  [1  (ntimes-i)*DT  0  0; 

0  1  0  0; 

0  0  1  (ntimes-i)*DT; 

0  0  0  1]; 

GG  =  [1  0  0  0; 

0  100; 

00  10; 

0  00  1]; 

QQ =  [((ntimes-i)*DT)A2*SIGMA_XDOTA2  0  0  0; 

0  SIGMA_XDOTA2  0  0; 

0  0  )(ntimes-i)*DT)A2*SIGMA_YDOTA2  0; 

0  0  0  SIGMA_YDOTA2]; 

MM  =  (inv(FF))'*(tracks(i,l).Yest)*(inv(FF)); 

S  Sigma  =  GG'*MM*GG+inv(QQ); 

OOmega  =  MM  *GG*inv(S  Sigma); 

projYpred  =  MM-(OOmega*SSigma*OOmega'); 

projYpred  =  forcesym(projYpred); 

projP  =  inv(projYpred); 

projPsigma  =  sqrt(projP); 

projxuncert  =  projPsigma)  1,1); 

projyuncert  =  projPsigma(3,3); 

proj  distuncert  =  sqrt(projxuncertA2  +  projyuncertA2); 

proj  ypred  =(eye(4,4)-(OOmega*  GG'))  *  (inv(FF))'  *  tracks(i,  1 )  .yest; 

projstate  =  proj  P*proj  ypred; 

warning  on; 

if  min(inpolygon  (b.X,  b.Y,  TSS_X,  TSS_Y))  ==  0  &&... 
min(inpolygon  (b.X,  b.Y,  PL_X,  PL  Y))  ==  0 

buf=sprintf('Warning:  Entered  Port  Limits  at  Time=%d’,i-l);disp(buf); 

%  Check  if  SAW  is  turning  towards  Jurong  Island 
disttoreach  =  sqrt(abs(x(l)-302.1 144444)A2  +... 

abs(x(3)-347.7716667)A2); 
speedtoreach  =  sqrt(x(2)A2+x(4)A2); 
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timetoreach  =  disttoreach/speedtoreach; 

%  Calculate  the  time  target  left  Port  Limits 
leftPL(i)  =  MAXTIME-timetoreach; 

%  Calculate  the  distance  between  the  projected  target  end  point 
%  and  Jurong  Island 

distapart  =  sqrt((projstate(l)  -  302.1 144444)A2  +  ... 

(projstate(3)  -  347.7716667)A2); 

%  Threat  level  assessment 

%  Send  warning  if  target  is  within  0.5nm  to  Jurong  Island 
if  distapart  <  5  &&  MAX  TIME  -  (i-l)*DT  >=  0  " 
buf=sprintf('Waming:  Heading  Towards  Jurong  Island.  Time  to  Impact  =  %.2f 
sec', timetoreach);  disp(buf); 

if  projdistuncert  >  distapart 

buf=sprintf(' Warning:  High  Confidence  of  hnpacf);disp(buf); 
startplot  =  i-1; 

leftPLntoJIHigh(i)  =  MAXTIME-timetoreach; 

else 

buf=sprintf(' Warning:  Moderate  Confidence  of  Impacf);disp(buf); 
startplot  =  i- 1 ; 

leftPLntoJIModerate(i)  =  MAXTIME-timetoreach; 
end 
end 

%  Check  if  SAW  has  left  Traffic  Separation  Scheme 
elseif  min(inpolygon  (b.X,  b.Y,  TSS_X,  TSS_Y))==  0  &&... 
min(inpolygon  (b.X,  b.Y,  PL_X,  PL  Y))  ==  1 
buf=sprintf('Warning:  Left  Traffic  Separation  Scheme  at  Time=%d',i-1); 
disp(buf); 

%  Check  if  SAW  is  turning  towards  Jurong  Island 
disttoreach  =  sqrt(abs(x(l)-302.1 144444)A2+... 

abs(x(3)-347.7716667)A2); 
speedtoreach  =  sqrt(x(2)A2+x(4)A2); 
timetoreach  =  disttoreach/speedtoreach; 

%  Calculate  the  time  target  left  TSS 
leftTSS(i)  =  MAXTIME-timetoreach; 

%  Calculate  the  distance  between  the  projected  target 

%  end  pointand  Jurong  Island 

distapart  =  sqrt((projstate(l)  -  302.1 144444) A2  +  ... 

(projstate(3)  -  347.7716667)A2); 

%  Threat  level  assessment 
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%  Send  warning  if  target  is  within  0.5nm  to  Jurong  Island 
if  distapart  <  5  &&  MAX  TIME  -  (i-l)*DT  >=  0  ^ 

buf=sprintf('Waming:  Heading  Towards  Jurong  Island.  Time  to  Impact  =  %.2f 
sec',timetoreach);  disp(buf); 

if  projdistuncert  >  distapart 

buf=sprintf(’ Warning:  High  Confidence  of  Impact’ );disp(buf); 
startplot  =  i-1; 

leftTSSntoJIHigh(i)  =  MAXTIME-timetoreach; 

else 

buf=sprintf(’ Warning:  Moderate  Confidence  of  Impact’ );disp(buf); 
startplot  =  i-1; 

leftTSSntoJIModerate(i)  =  MAXTIME-timetoreach; 
end 
end 
end 

kdata(l,i)  =  tracks(i,l).time; 
kdata(2,i)  =  projdistuncert*  180; 
kdata(3,i)  =  distapart*  180; 
kdata(4,i)  =  disttoreach*180; 
kdata(5,i)  =  speedtoreach*180; 
kdata(6,i)  =  timetoreach; 
kdata(7,i)  =  startplot; 
kdata(8,i)  =  projP(l,l); 
kdata(9,i)  =  projP(3,3); 
kdata(10,i)  =  projstate(l); 
kdata(l  l,i)  =  projstate(3); 

end 

%  Future  work  for  multi-targets 
%  Search  for  first  observation  for  other  targets 
if  NUM  TARGETS  ~=1 

for  target=2  :NUM_T  ARGETS 
for  k  =  1  msensors 
forj  =l:ntimes; 

if  (~isnan(observations(k,j).report(  target, 2:5))) 
temp(j)=j; 
else 

temp(j)=99999; 

end 

end 

starttime(k)  =  min(temp); 
if  (starttime(k)  <=  starttime(l)) 
startstep  =  starttime(k); 
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end 

end 

end 

end 

%  Initialisation 

for  j=2:NUM_TARGETS 

tracks(startstep,j)=init_itracks(j, observations, fdter,startstep); 
end 

%  Track 

for  i=(startstep+l):ntimes 
%  Prediction  for  each  target 
for  j=2  :NUM_TARGETS 
tracks(i,j)=info_pred(tracks(i- 1  ,j),times(i), filter); 
end 

%  Data  association,  returning  an  obs*track  array  of  associations 
da_array=data_association(  tracks, observations, i); 

%  Update  for  each  target 
for  j=2  :NUM_TARGETS 

tracks(i,j)=info_update(tracks(i,j), observations, filter, da_array,i,j); 
end 
end 
end 

figure(20); 

elf; 

plot(kdata(l,:),(kdata(2,:)),'b’,kdata(l,:),kdata(3,:),'g') 
legend(’Distance  Uncertainty',' Distance  Apart'); 
buf=sprintf('Estimated  End  Point'); 
title(buf) 

xlim([min(kdata(7, 1 000:MAX_TIME/DT))*DT  MAX  TIME- 1  *DT]) 
axis  'auto  y' 

xlabel(’Simulated  Time  (s)') 
ylabel('Distance  (m)') 

figure(21); 

elf; 

plot(kdata(l,:),kdata(4,:),'b’,kdata(l,:),kdata(5,:),'g',kdata(l,:),kdata(6,:),'r') 
legend('Distance  to  Impact', 'Speed  to  Impact',  'Time  to  Impact'); 
buf=sprintf('Projected  Distance,  Speed  and  Time  to  Impact'); 
title(buf) 

xlim( [min(kdata(7, 1 000:MAX_TIME/DT))*DT  MAX  TIME- 1  *DT]) 
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axis  'auto  y' 

xlabel('Simulated  Time  (s)') 

%  Record  the  times  the  target  crossed  TSS,  PL,  &  corresponding  threat  level 

firstleftTSS  =  min(leftTSS(1000:MAX_TIME/DT)); 

firstleftT  S  Snto  JIHigh  =  min(leftTSSntoJIHigh(1000:MAX_TIME/DT)); 

lirstlcftTSSn  to  J I M  odcratc  =  min(leftTSSntoJIModerate(1000:MAX_TIME/DT)); 

firstleftPL  =  min(leftPL(  1 000  :MAX_TIME/DT)); 

firstleftPLntoJIHigh  =  min(leftPLntoJIHigh(1000:MAX_TIME/DT)); 

firstleftPLnto  JIModerate  =  min(leftPLntoJIModerate(  1 000  :MAX_TIME/DT)); 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  initialises  a  set  of  tracks  from  the  first  few  observations 
function  track=init_itracks(ntarget, observations, filter, startstep) 
globals; 

%  Straight-forward  in  infonnation  form,  now  initialise  the  track 

ypred=zeros(XSIZE,l);  %initial  condition 

yest=ypred; 

Ypred=zeros(XSIZE,XSIZE);  %initial  condition 
Yest=Ypred; 

%  put  together  data  structure 

track=make_itrack(ntarget,  1 , startstep, ypred,yest,Ypred,Y  est); 
function  track=make_track(id, type, time, ypred,yest,Ypred,Y est) 

track=struct('id', id, 'type', type, 'time', time, ’ypred',ypred,'yest',yest,'Ypred',Ypred,'Yesf,Yest 

); 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  performs  prediction  for  a  CVM 

function  track_pred=info_pred(track, time, filter) 

dt  =  time-track.time; 
if  dt  <  0 

error('Negative  prediction  step  in  state_pred’) 
end 

%  Compute  transition  matrices 
F=[l  dt  0  0; 

0  100; 

0  0  1  dt; 

00  0  1]; 

G=[l  0  0  0; 

0  100; 

00  10; 

00  0  1]; 

Q=filter.Q; 

%  Do  prediction 

M=(inv(F))'  *  (track.  Y est)  *  (inv(F)); 

S  igma=G' *  M  *  G+inv(Q) ; 

Omega=M*G*inv(Sigma);  %  here  is  the  problem 
Ypred=M-(Omega*Sigma*Omega'); 
ypred=(eye(4,4)-(Omega*G'))*(inv(F))'*(track.yest); 
Ypred=force_sym(Ypred);  %  seems  to  require  for  stability  in  Matlab 

%  Make  space  for  later 

Yest=Ypred; 

yest=ypred; 

%  Construct  the  new  track 

track_pred=make_itrack(track.id,  1  ,time,ypred,yest,Ypred,Y  est); 
function  sigma=force_sym(sigma) 

% 

%  a  function  that  forces  symmetry  on  a  matrix 
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% 


[nx,ny]=size(sigma); 
if  nx  ~=  ny 

error('Matrix  must  be  square'); 
end 

for  i=l:nx 
for  j=i+l:nx 

temp=(sigma(i,j)+sigma(j,i))/2; 

sigma(i,j)=temp; 

sigma(j,i)=temp; 

end 

end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  sets  up  the  data  association  array 

function  da=data_association(tracks, observations, time) 

%  This  is  an  array  of  size  ntargets  to  nsensors 

%  For  each  sensor  i,  the  entry  states  which  sensor  report 

%  is  associated  with  a  target  j 

[nsensors,  ntimes]=size(observations); 

[ntimes,ntargets]=size(tracks); 

da=zeros(ntargets, nsensors); 

%  For  each  sensor 
for  i=l:nsensors 

%  Associate  a  track  with  a  report 
for  j=l:ntargets 
da(j  ,i)=j ; 
end 
end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  perfonns  information  filter  update 

function  track=info_update(track, observations, filter, da_array,ntime,ntarget) 
globals; 

%  Inititialise  to  prediction 
yest=track.ypred; 

Yest=track.Ypred; 

%  Observation  model 
H=filter.H; 

%  Get  new  information 
[nsensors,temp]=size(observations); 
for  i=l:nsensors 

if  filter.use_sensors(i)  ==  1 

%  Check  for  valid  observations  first 
if  (~isnan(observations(i,ntime).report(ntarget,2:5))) 
if  (observations^, ntime)  .report(ntarget,2 : 5  )~=99999) 

%  Extract  observation 
rep=observations(i,ntime). report; 
na=da_array(ntarget,i); 

[zx,zy,R]=xy_obs(rep,na,i);  %  R  is  unused 

z=[zx;zy]; 

if  i==l 

Ri=inv(Rfilter_MPA); 
elseif  i==2 

Ri=inv(Rfilter_MP  A) ; 
elseif  i==3 

Ri=inv(Rfilter_MP  A) ; 
elseif  i==4 

Ri=inv(Rfilter_MP  A) ; 
elseif  i==5 

Ri=inv(Rfilter_MP  A) ; 
elseif  i==6 

Ri=inv(Rfilter_PCG); 
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elseif  i==7 

Ri=inv(Rfilter_PCG); 
elseif  i==8 

Ri=inv(Rfilter_PV); 
elseif  i==9 

Ri=inv(Rfilter_MPAC); 

end 

%  Construct  information  terms 
info=H'*Ri*z; 

Info=H'*Ri*H; 

%  Add  the  information  to  prediction 
yest=yest+info; 

Y  est= Y  est+Info ; 

%  Seems  to  require  for  stability  in  Matlab 

Y  est=force_sym(Y  est); 

elseif  (observations(i,ntime).report(ntarget,2:5)==  99999) 
yest=yest; 

Yest=Yest; 

%  Seems  to  require  for  stability  in  Matlab 

Y  est=force_sym(Y  est); 
end 

end 

end 

end 

%  Do  assignment 
track.yest=yest; 
track.  Yest=Yest; 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  file  displays  the  fused  target  track  following  a  simulation  run 
globals; 

%  Set  plotting  size 

[nsamps,ntargets]=size(tracks); 

figure(l) 

xlim([150  750]) 

ylim(  [280  440]) 

hold  on 

warning  off; 

data=zeros(2,nsamps); 

j  =i; 

while  (isempty(tracks(j, l).ypred(:))) 

j  =  j+i; 

end 

trackstarttime  =  j; 

for  n=l:ntargets 
for  i=trackstarttime:nsamps 
Y=tracks(i,n).Yest; 

P=inv(Y); 

x=P  *  tracks(i,n) .  yest; 
data(l,i)=x(l); 
data(2,i)=x(3); 
end 

estip=plot(data(l,trackstarttime:nsamps),data(2,trackstarttime:nsamps),'g-'); 

end 

legend([estip],  'Estimated  Track  Position') 
hold  off 

plot_errors(tracks,  ntargets,  2,  trackstarttime,  targets) 
warning  on; 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  produces  various  plots  with  regards  to  the  SAW  parameters 
function  plot_errors(tracks,ntarget,nfigure, trackstarttime, targets) 
globals; 

%  Initialise 

[nsamps,ntargets]=size(tracks); 
ydata=zeros(  1 0,nsamps); 
pdata=zeros(10,nsamps);  %predicted  data 
edata=zeros(10,nsamps);  %estimated  data 
tdata=zeros(10,nsamps);  %true  data 
esttimesigma=zeros(  1  ,nsamps); 
xmin  =  4500; 
xmax  =  MAXTIME; 

warning  off; 

%  Calculating  and  storing  the  data  for  each  time-step 
for  i=trackstarttime:nsamps 

Y=tracks(i,ntarget).  Y  est; 

P=inv(Y); 

x=P*tracks(i,ntarget).yest; 

Y  p=tracks(i,ntarget) .  Ypred; 

Pp=inv(Yp); 

xp=Pp*tracks(i,ntarget).  ypred; 

ydata(  1  ,i)=tracks(i,ntarget).time; 

ydata(2,i)=(x(  l)-targets(i,ntarget).x)*  1 80; 

ydata(3,i)=(x(3)-targets(i,ntarget).y)*180; 

ysigma=real(sqrt(P)); 

ydata(4  ,i)=ysigma(  1 , 1 )  *  1 8  0 ; 

ydata(  5  ,i)=ysigma(3 , 3  )  *  1 8  0 ; 

ydata(6,i)=(real(sqrt(x(2)*x(2)+x(4)*x(4)))-targets(i,ntarget).vel)*180; 
ydata(7,i)=atan2(x(4),x(2))-targets(i,ntarget).phi; 
if  abs(ydata(7,i))  >  pi 
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ydata(7,i)  =  2*pi-abs(ydata(7,i)); 

end 

ydata(8,i)=sqrt(ysigma(  1 , 1  )*ysigma(  1 , 1  )+ysigma(3,3)*ysigma(3,3))*  1 80; 
ydata(9,i)=sqrt(ysigma(2,2)*ysigma(2,2)+ysigma(4,4)*ysigma(4,4))*180; 
ydata(10,i)=real(sqrt(((x(2)/(x(2)A2  +  x(4)A2))A2)*(ysigma(4,4)A2)... 
+((x(4)/(x(2)A2  +  x(4)  A2))A2)*  (ysigma(2 ,2)A2))); 

tdata(  1  ,i)=tracks(i,ntarget).time; 
tdata(2,i)=targets(i,ntarget).x*  1 80; 
tdata(3,i)=targets(i,ntarget).y*  1 80; 
tdata(6,i)=targets(i,ntarget).vel*  1 80; 
tdata(7,i)=targets(i,ntarget).phi; 

pdata(  1  ,i)=tracks(i,ntarget).time; 
pdata(2,i)=xp(  1)*  1 80; 
pdata(3,i)=xp(3)*180; 
psigma=real(sqrt(Pp)); 
pdata(4,i)=psigma(  1 , 1)*  1 80; 
pdata(5,i)=psigma(3,3)*  180; 

pdata(6,i)=(real(sqrt(xp(2)*xp(2)+xp(4)*xp(4))))*  1 80; 
pdata(7,i)=atan2(xp(4),xp(2)); 
if  abs(pdata(7,i))  >  pi 
pdata(7,i)  =  2*pi-abs(pdata(7,i)); 
end 

pdata(8,i)=sqrt(psigma(l,l)*psigma(l,l)+psigma(3,3)*psigma(3,3))*180; 
pdata(9,i)=sqrt(psigma(2,2)*psigma(2,2)+psigma(4,4)*psigma(4,4))*180; 
pdata(10,i)=real(sqrt(((xp(2)/(xp(2)A2  +  xp(4)A2))A2)*(psigma(4,4)A2)... 
+((xp(4)/(xp(2)A2  +  xp(4)A2))A2)*(psigma(2,2)A2))); 

edata(  1  ,i)=tracks(i,ntarget).time; 
edata(2,i)=x(l)*180; 
edata(3,i)=x(3)*180; 
esigma=real(sqrt(P)); 
edata(4,i)=esigma(  1 , 1)*  1 80; 
edata(5,i)=esigma(3,3)*180; 
edata(6,i)=(real(sqrt(x(2)*x(2)+x(4)*x(4))))!|5 1 80; 
edata(7,i)=atan2(x(4),x(2)); 
if  abs(edata(7,i))  >  pi 
edata(7,i)  =  2*pi-abs(edata(7,i)); 
end 

edata(8  ,i)=sqrt(esigma(  1,1)*  esigma(  1 , 1  )+esigma(3 ,3)  *  esigma(3 ,3  ))  *  1 80 ; 
edata(9,i)=sqrt(esigma(2,2)*esigma(2,2)+ysigma(4,4)5|5esignia(4,4))5|!180; 
edata(10,i)=real(sqrt(((x(2)/(x(2)A2  +  x(4)A2))A2)*(esigma(4,4)A2)... 
+((x(4)/(x(2)A2  +  x(4)A2))A2)*(esigma(2,2)A2))); 
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esttimesigma(  1  ,i)=sqrt(edata(8,i)A2+. . . 
(kdata(6,i)A2)*(edata(9,i)A2))/... 
edata(6,i); 
end 

%  merging  the  heading  standard  deviations  for  plotting 
p=tracks  tarttime ;  q=trackstarttime;  i=tracks  tarttime ; 
for  j = 1 :  (nsamps-trackstarttime+ 1 )  *  2 
if  j/2-round(j/2)==0 

combtimedata(  1  j)=  tracks(i,ntarget).time; 

combdata(8,j)=edata(8,p); 

combdata(9,j)=edata(9,p); 

combdata(  1 0,j)=edata(  1 0,p); 

p=p+l; 

i=i+l; 

else 

combtimedata(  1  j)=  tracks(i,ntarget).time; 
combdata(8,j)=pdata(8,q); 
combdata(9,j)=pdata(9,q); 
combdata(  1 0,j)=pdata(  1 0,q); 
q=q+l; 
end 
end 

%  Plot 

figure(nfigure); 

elf; 

plot(ydata(  1  ,:),abs(ydata(2,:)),'b’,ydata(  1  ,:),ydata(4,:),'r') 
legend(’Estimated  Error’,’Standard  Deviation’); 
buf=sprintf(’Error  between  X-component  Estimate  and  Truth’); 
title(buf) 

xlim([xmin  xmax]) 
axis  ’auto  y’ 

xlabel(’Simulated  Time  (s)’) 
ylabel(’X  error  (m)’) 

figure(nfigure+ 1 ) ; 
elf; 

plot(ydata(  1  ,:),abs(ydata(3,:)),’b’,ydata(  1  ,:),ydata(5  ,:),T) 
legend(’Estimated  Error’, ’Standard  Deviation’); 
buf=sprintf(’Error  between  Y-component  Estimate  and  Truth’); 
title(buf) 

xlim([xmin  xmax]) 
axis  ’auto  y’ 

xlabel(’Simulated  Time  (s)’) 
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ylabel(’Y  error  (m)') 

figure(nfigure+2) ; 
elf; 

plot(ydata(l,:),abs(ydata(6,:)),'b’,ydata(l,:),ydata(9,:),'r') 
legend(’Estimated  Error’, ’Standard  Deviation’); 
buf=sprintf(’Error  between  Velocity  Estimate  and  Truth’); 
title(buf) 

xlim([xmin  xmax]) 
axis  'auto  y’ 

xlabel(’Simulated  Time  (s)’) 
ylabel(’Velocity  error  (m/s)’) 

figure(nfigure+3 ) ; 
elf; 

plot(ydata(  1  ,:),abs(ydata(7,:)),’b’,ydata(  1  ,:),ydata(  1 0,:),T’) 
legend(’Estimated  Error’, ’Standard  Deviation’); 
buf=sprintf(’Error  between  Heading  Estimate  and  Truth’); 
title(buf) 

xlim([xmin  xmax]) 
axis  'auto  y’ 

xlabel(’Simulated  Time  (s)’) 
ylabel(’Heading  error  (rads)’) 

figure(nfigure+4) ; 
elf; 

plot(pdata(l,:),pdata(2,:),’r’,edata(l,:),edata(2,:),’g’,  tdata(l,:),tdata(2,:),’b’) 
legend(’Predicted  X’, ’Estimated  X’,’True  X’); 
buf=sprintf(’X  position’); 
title(buf) 

xlim([xmin  xmax]) 
axis  ’auto  y’ 

xlabel(’Simulated  Time  (s)’) 
ylabel(’X  Position  (m)’) 

figure(nfigure+5  ) ; 
elf; 

plot(pdata(l,:),pdata(3,:),’r’,edata(l,:),edata(3,:),'g’,  tdata(l,:),tdata(3,:),’b’) 
legend('Predicted  Y’, ’Estimated  Y’,’True  Y’); 
buf=sprintf('Y  position’); 
title(buf) 

xlim([xmin  xmax]) 
axis  'auto  y' 

xlabel('Simulated  Time  (s)') 
ylabel('Y  Position  (m)') 
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figure(nfigure+6) ; 
elf; 

plot(pdata(l,:),pdata(6,:),’r’,edata(l,:),edata(6,:),’g’,  tdata(l,:),tdata(6,:),'b’) 
legend('Predicted  VelocityVEstimated  Velocity','True  Velocity'); 
buf=sprintf(’V  elocity'); 
title(buf) 

xlim([xmin  xmax]) 
axis  'auto  y' 

xlabel('Simulated  Time  (s)') 
ylabel('Velocity  (m/s)’) 

figure(nfigure+7) ; 
elf; 

plot(pdata(l,:),pdata(7,:),'r',edata(l,:),edata(7,:),’g’,  tdata(l,:),tdata(7,:),’b') 
legend('Predicted  H cadi n g E st i m atcd  Heading',' True  Heading'); 
buf=sprintf('Heading'); 
title(buf) 

xlim([xmin  xmax]) 
axis  'auto  y' 

xlabel('Simulated  Time  (s)') 
ylabel('Heading  (rad)') 

figure(nfigure+8); 

elf; 

plot(pdata(l,:),pdata(8,:),'r+',edata(l,:),edata(8,:),'g*',combtimedata(l,:),combdata(8,:),’b’) 
legend('Predicted', 'Estimated'); 
buf=sprintf('Radius  Standard  Deviations’); 
title(buf) 

xlim([xmin  xmax]) 
axis  'auto  y' 

xlabel(’Simulated  Time  (s)') 
ylabel('Radius  (m)') 

figure(nfigure+9) ; 
elf; 

plot(pdata(l,:),pdata(9,:),'r+',edata(l,:),edata(9,:),'g*',combtimedata(l,:),combdata(9,:),’b’) 

legend('Predicted','Estimated’); 

buf=sprintf('Velocity  Standard  Deviations’); 

title(buf) 

xlim([xmin  xmax]) 
axis  'auto  y' 

xlabel(’Simulated  Time  (s)') 
ylabel('Velocity  (m/s)') 
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figure(nfigure+ 1 0); 
elf; 

plot(pdata(  1  ,:),pdata(  10,:  ),'r+',edata(  1 ,  :),edata(  1 0,:),'g*  ',combtimedata(  1 ,  :),combdata(  1 0, 

),’b’) 

legend('PredictedVEstimated'); 
buf=sprintf(’Heading  Standard  Deviations'); 
title(buf) 

xlim([xmin  xmax]) 
axis  'auto  y' 

xlabel('Simulated  Time  (s)') 
ylabel('Heading  (rad)') 

figure(nfigure+l  1); 
elf; 

plot(edata(  1  ,:),abs((MAX_TIME-edata(l  ,:))- 
kdata(6 , : )) , 'b ', edata(  1 , : ), esttimesigma(  1 , : ), 'r' ) 
legend('Estimated  Error’, 'Standard  Deviation'); 
buf=sprintf('Error  between  Time  Estimate  and  Truth'); 
title(buf) 

xlim( [min(kdata(7, 1 000:MAX_TIME/ 1 0))*DT  xmax- 1  *DT]) 
axis  'auto  y' 

xlabel('Simulated  Time  (s)') 
ylabel('Time  (s)') 

warning  on; 
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C.  BLOCK  3.  DECENTRALIZED  DATA  FUSION  ARCHITECTURE  USING 
INFORMATION  FILTER 

%  This  file  executes  the  Monte-Carlo  simulation 

%  Number  of  Monte-Carlo  simulation  runs 
totalruns  =  50; 

%  Initialise  variables 
sumfirstleftTSS(l  Totalruns)  =  NaN; 
sumfirstleftTSSntoJIHigh(l  Totalruns)  =  NaN; 
sumfirstleftTSSntoJIModerate(  1  Totalruns)  =  NaN; 
sumfirstleftPL(  1  Totalruns)  =  NaN; 
sumfirstleftPLnto  JIHigh(  1  Totalruns)  =  NaN; 
sumfirstleftPLntoJIModerate(l  Totalruns)  =  NaN; 

%  Execute  each  run  by  generating  new  observations  or  using  previous  ones 
%  Produce  the  fused  track  for  each  run 
%  Then  pool  the  tracks  together  for  analysis 
for  run  =1  Totalruns 

buf=sprintf('Run:  =%d',run);  disp(buf); 
example_sim;  %generate  new  observations 

observations  =  datastore5(run). observations;  %  using  previous  observations 

NEW_RUN  =  0; 

runnet; 

sumfirstleftTSS(run)  =  firstleftTSS; 
sumfirstleftTSSntoJIHigh(run)  =  firstleftTSSntoJTHigh; 
sumfirstleftTSSntoJIModerate(run)  =  firstleftTSSntoJIModerate; 
sumfirstleftPL(run)  =  firstleftPL; 
sumfirstleftPLntoJIHigh(run)  =  firstleftPLntoJIHigh; 
sumfirstleftPLntoJIModerate(run)  =  firstleftPLntoJIModerate; 
sumkdata(run,:,:)  =  kdata; 
sumesttimesigma(run,l,:)  =  esttimesigma; 
lowest(run)  =  min(sumkdata(run,7,1000:MAX_TIME/10)); 
end 

%  Calculate  Probability  of  Impact 
Cxtemp  =  squeeze(sumkdata(:,8,:)); 

Cytemp  =  squeeze(sumkdata(:,9,:)); 

Xtemp  =  squeeze(sumkdata(:,10,:)); 

Ytemp  =  squeeze(sumkdata(:,l  1,:)); 
probability  =  zeros(l,nt); 

warning  off; 
for  1  =  1  :nt 
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Cx  =  diag(Cxtemp(:,l))*(180A2); 

Cy  =  diag(Cxtemp(:,l))*(180A2); 

X  =  (Xtemp(:,l)-  302.1 144444)*  180; 

Y  =  (Ytemp(:,l)-  347.7716667)*  180; 

W  =  ones(totalruns,l); 
sigma_xbar_tgo  =  inv((W')*inv(Cx)*W); 
sigma_ybar_tgo  =  inv((W')*inv(Cy)*W); 
sigma  =  sqrt(sigma_xbar_tgo); 

xbar_tgo  =  sigma_xbar_tgo*(((W')*inv(Cx)*X));%  Calculate  mean  impact  pt 
ybar_tgo  =  sigma_ybar_tgo*(((W')*inv(Cy)*Y));%  Calculate  mean  impact  pt 
r_not  =  sqrt(xbar_tgoA2  +  ybar_tgoA2); 

R  =  300; 

g  =  zeros(l,100); 
h  =  zeros(l,100); 
probtemp  =  0; 

recur  =  1 ; 

g_not  =  exp(-(r_notA2)/(2*sigmaA2));  %  g_not 
h_not  =  1  -  exp(-(RA2)/(2*sigmaA2));  %  h  not 
probtempnot  =  g_not*h_not; 

g(l)  =  (l/l)*((r_notA2)/(2*sigmaA2))*g_not; 

h(l)  =  -(l/factorial(l))*(((RA2)/(2*sigmaA2))A(l))*exp(-(RA2)/(2*sigmaA2))  +  h_not; 
probtemp  =  probtempnot  +  g(l)*h(l); 

while  (recur  <=  100) 

g(recur+l)  =  (l/(recur+l))*((r_notA2)/(2*sigmaA2))*g(recur); 

h(recur+ 1 )  =  (- 1  /  factorial(recur+ 1  ))*  (((RA2)/ (2  *  sigmaA2))A(recur+ 1 ))  *  exp(- 

(RA2)/(2*sigmaA2))  +  h(recur); 

probtemp  =  probtemp  +  g(recur+l)*h(recur+l); 
recur  =  recur  +  1 ; 
end 

probability(l,l)  =  probtemp; 
end 

%  Initialise  variables 
count  1  =  0; 
count2  =  0; 
count3  =  0; 
count4  =  0; 
count5  =  0; 
count6  =  0; 
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SsumfirstleftTSS  =  0; 

Ssumfi  rstlcftTSSntoJIHigh  =  0; 

SsumfirstleftTSSntoJIModerate  =  0; 

Ssumfi  rs  tic  ft  PL  =  0; 

SsumfirstleftPLntoJIHigh  =  0; 

SsumfirstleftPLntoJIModerate  =  0; 

%  Take  the  average  of  the  pooled  tracks 
for  m  =  1  :totalruns 

if  ~isnan(sum  (1  rstlcftTSS(m)) 
count  1  =  countl+1; 

SsumfirstleftTSS=SsumfirstleftTSS+sumfirstleftTSS(m); 

end 

if  ~isnan(sumfirstleftTSSntoJIHigh(rn)) 
count2  =  count2+l; 

SsumfirstleftTSSntoJIHigh=SsumfirstleftTSSntoJIHigh+surnfirstleftTSSntoJIHigh(rn); 

end 

if  ~isnan(sumfirstleftTSSntoJIModerate(m)) 
count3  =  count3+l; 

SsumfirstleftTSSntoJIModerate=SsumfirstleftTSSntoJIModerate+surnfirstleftTSSntoJIM 

oderate(m); 

end 

if  ~isnan(sumfirstlcftPL(m)) 
count4  =  count4+l; 

S  sumfirstleftPL=S  sumfirstleftPL+sumfirstleftPL(m) ; 
end 

if  ~isnan(sumfirstleftPLntoJIHigh(m)) 
count5  =  count5+l; 

SsumfirstleftPLntoJIHigh=SsumfirstleftPLntoJIHigh+sumfirstleftPLntoJIHigh(m); 

end 

if  ~isnan(sumfirstleftPLntoJIModerate(m)) 
count6  =  count6+l; 

SsumfirstleftPLntoJIModerate=SsumfirstleftPLntoJIModerate+sumfirstleftPLntoJIModer 

ate(m); 

end 

end 
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mean  fir stlc RTSS  =  SsumfirstleftTSS/countl; 

mean  llrstlcftTSSnloJl  High  =  Ssum  (1  rstlcf\TSSntoJIHigh/count2; 

m  can  11  rstl  c  ftTS  S  n  to  J I M  o  derate  =  S  s  um  (1  rs  1 1  c  ft  T  S  S  n  t  o  J I M  o  dc  ratc/c  o  un  t3 ; 

meanfirstleftPL  =  SsumfirstleftPL/count4; 

meanfirstleftPLntoJIHigh  =  Ssum  firstlcftP  LntoJ  1  High/count5 ; 

meanfirstleftPLnto  JIModerate  =  Ssum  UrstleftP  LntoJ  I  Moderate/  count6; 

meantruetimedata  =  mean(sumkdata(:,l,:)); 

meandistuncert  =  mean(sumkdata(:,2,:)); 

meandistapart  =  mean(sumkdata(:,3,:)); 

meanesttimedata  =  mean(sumkdata(:,6,:)); 

%  Calculate  the  sample  variance  of  the  estimated  time  to  impact 
for  p  =  1  :nt 
sumtimediff=0; 
timediff=0; 
for  q  =  1 :  total  runs 

timediff  =  (sumkdata(q,6,p)-meanesttimedata(p))A2 ; 
sumtimediff=sumtimediff+timediff; 
end 

meansampletimesigma(p)  =  sqrt(sumtimediff  /(totalruns-1)); 
end 

ftgure(30); 

elf; 

plot(meantruetimedata(:)',meandistuncert(:)','b',meantruetimedata(:)',meandistapart(:)Vg') 
legend(’Distance  Uncertainty', 'Distance  Apart'); 
buf=sprintf('Estimated  End  Point'); 
title(buf) 

xlim([min(lowest)*DT  MAX  TIME- 1  *DT]) 
ylim(  [0  1000]) 
xlabel(’Simulated  Time  (s)') 
ylabel(’Distance  (m)') 

figure(31); 

elf; 

plot(meantruetimedata(:)',abs((MAX_TIME-meantruetimedata(:)')- 
meanesttimedata(:)'),'b',meantruetimedata(:)',meansampletimesigma,’r' ) 
legend('Estimated  Error', 'Standard  Deviation'); 
buf=sprintf('Error  between  Time  Estimate  and  Truth'); 
title(buf) 

xlim([min(lowest)*DT  MAX  TIME- 1  *DT]) 
ylim([0  100]) 

xlabel(’Simulated  Time  (s)') 
ylabel('Time  (s)') 
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figure(32); 

elf; 

plot(meantruetimedata(:)',probability(:),’k’) 
legend('Probability  of  Impact'); 
buf=sprintf('Probability  of  Impact’); 
title(buf) 

xlim([min(lowest)*DT  MAX  TIME- 1  *DT]) 
ylim([0  1]) 

xlabel(’Simulated  Time  (s)') 
ylabel('Probability  of  Impact') 
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This  file  executes  the  Monte-Carlo  simulation  and  incorporates  dataloss 
%  (i.e.  observations  loss)  due  to  communication  bandwidth  limitations 

%  Number  of  Monte-Carlo  simulation  runs 
totalruns  =  50; 

%  Initialise  variables 
sumfirstleftTSS(l  Totalruns)  =  NaN; 
sumfirstleftTSSntoJIHigh(l  Totalruns)  =  NaN; 
sumfirstleftTSSntoJIModerate(l  Totalruns)  =  NaN; 
sumfirstleftPL(l  Totalruns)  =  NaN; 
sumfirstleftPLnto  JIHigh(  1  Totalruns)  =  NaN; 
sumfirstleftPLntoJIModerate(l  Totalruns)  =  NaN; 

%  Execute  each  run  by  generating  new  observations  or  using  previous  ones 
%  Inject  data  loss 

%  Produce  the  fused  track  for  each  run 
%  Then  pool  the  tracks  together  for  analysis 
for  run  =1  Totalruns 

buf=sprintf(’Run:  =%d',run);  disp(buf); 
example_sim;  %generate  new  observations 

%observations  =  datastoreloss(run). observations;  %  using  previous  observations 

%  Since  previous  observations  (with  loss)  is  used,  there  is  no  need  to 
%  re-inject  data  loss.  If  new  observations  (with  no  loss)  are  made,  the 
%  following  codes  are  necessary  to  inject  data  loss. 

% - 

%  simulate  data  loss  due  to  bandwidth  limitations 
for  i  =  1:10 

j  =  randi(MAX_TIME/DT- 1060-3)+ 1060; 

datalossduration  =  randi(3); 
for  k  =  1 :  datalossduration 

for  n  =  1  :NUM_PLATFORMS 
observations(n,j+k).report(2:5)  =99999; 
end 
end 
end 

datastoreloss(run).observations=observations; 

% - 

NEW_RUN  =  0; 
runnet; 

sumfirstleftTSS(run)  =  firstleftTSS; 
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sumfirstleftTSSntoJIHigh(run)  =  firstleftTSSntoJIHigh; 
sum  (1  rstlcftTSSntoJIModcratc(run)  =  (1  rstlcftTSSnto.il  Moderate; 
sumfl  rstleftPL(run)  =  firstleftPL; 
sumfirstleftPLntoJIHigh(run)  =  firstleftPLntoJIHigh; 
sumfirstleftPLntoJIModerate(run)  =  fi  rstl  c  ft  P  L  n  to  J I M  odcratc ; 
sumkdata(run,:,:)  =  kdata; 
sumesttimesigma(run,l,:)  =  esttimesigma; 
lowest(run)  =  min(sumkdata(run,7,1000:MAX_TIME/10)); 
end 

%  Calculate  Probability  of  Impact 
Cxtemp  =  squeeze(sumkdata(:,8,:)); 

Cytemp  =  squeeze(sumkdata(:,9,:)); 

Xtemp  =  squeeze(sumkdata(:,10,:)); 

Ytemp  =  squeeze(sumkdata(:,l  1,:)); 
probability  =  zeros(l,nt); 

warning  off; 
for  1  =  1  :nt 

Cx  =  diag(Cxtemp(:,l))*(180A2); 

Cy  =  diag(Cxtemp(:,l))*(180A2); 

X  =  (Xtemp(:,l)-  302.1 144444)*  180; 

Y  =  (Ytemp(:,l)-  347.7716667)*  180; 

W  =  ones(totalruns,l); 
sigma_xbar_tgo  =  inv((W')*inv(Cx)*W); 
sigma_ybar_tgo  =  inv((W')*inv(Cy)*W); 
sigma  =  sqrt(sigma_xbar_tgo); 

xbar_tgo  =  sigma_xbar_tgo*(((W')*inv(Cx)*X));%  Calculate  mean  impact  pt 
ybar_tgo  =  sigma_ybar_tgo*(((W')*inv(Cy)*Y));%  Calculate  mean  impact  pt 
r_not  =  sqrt(xbar_tgoA2  +  ybar_tgoA2); 

R  =  300; 

g  =  zeros(l,100); 
h  =  zeros(l,100); 
probtemp  =  0; 

recur  =  1 ; 

g_not  =  exp(-(r_notA2)/(2*sigmaA2));  %  g_not 
h_not  =  1  -  exp(-(RA2)/(2*sigmaA2));  %  h_not 
probtempnot  =  g_not*h_not; 

g(l)  =  (l/l)*((r_notA2)/(2*sigmaA2))*g_not; 

h(l)  =  -(l/factorial(l))*(((RA2)/(2*sigmaA2))A(l))*exp(-(RA2)/(2*sigmaA2))  +  h_not; 
probtemp  =  probtempnot  +  g(l)*h(l); 
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while  (recur  <=  100) 

g(recur+l)  =  (l/(recur+l))*((r_notA2)/(2*sigmaA2))*g(recur); 

h(recur+ 1 )  =  (- 1  /  factorial(recur+ 1  ))*  (((RA2)/ (2  *  s  i  g  m  a A  2 ) )  A(  r  c  c  u  r+ 1 ))  *  exp(- 

(RA2)/(2*sigmaA2))  +  h(recur); 

probtemp  =  probtemp  +  g(recur+l)*h(recur+l); 
recur  =  recur  +  1 ; 
end 

probability(l,l)  =  probtemp; 
end 

%  Initialise  variables 
count  1  =  0; 
count2  =  0; 
count3  =  0; 
count4  =  0; 
count5  =  0; 
count6  =  0; 

SsumfirstlcftTSS  =  0; 

S  sum  (i  rs  tl  c  1  IT  S  S  n  to  J  IHigh  =  0; 

Ssumfi  rstlcftTSSnto.il  Moderate  =  0; 

SsumfirstlcftPL  =  0; 

SsumfirstleftPLntoJIHigh  =  0; 

SsumfirstleftPLntoJIModerate  =  0; 

%  Take  the  average  of  the  pooled  tracks 
for  m  =  1  :totalruns 

if  ~isnan(sumfi  rstlcftTSS(m)) 
count  1  =  count  1+1; 

SsumfirstleftTSS=SsumfirstleftTSS+sumfirstleftTSS(m); 

end 

if  ~isnan(sumfirstleftTSSntoJIHigh(m)) 
count2  =  count2+l; 

SsumfirstleftTSSntoJIHigh=SsumfirstleftTSSntoJIHigh+sumfirstleftTSSntoJIHigh(m); 

end 

if  ~isnan(sumfirstleftTSSntoJIModerate(m)) 
count3  =  count3+l; 

SsumfirstleftTSSntoJIModerate=SsumfirstleftTSSntoJIModerate+sumfirstleftTSSntoJIM 

oderate(m); 

end 
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if  ~i  snan(  sum  (i  rstl  c  IIP  L(  m ) ) 
count4  =  count4+l; 

S  sumfirstleftPL=S  sumfirstleftPL+sumfirstleftPL(m) ; 
end 

if  ~isnan(sumfirstleftPLntoJIHigh(m)) 
count5  =  count5+l; 

SsumfirstleftPLntoJIHigh=SsumfirstleftPLntoJIHigh+sumfirstleftPLntoJIHigh(m); 

end 

if  ~i  snan(sum  li  rstl  c  ftP  Ln  to  J I M  oderatc(m )) 
count6  =  count6+l; 

SsumfirstleftPLntoJIModerate=SsumfirstleftPLntoJIModerate+sumfirstleftPLntoJIModer 

ate(m); 

end 

end 

m can  li  rstl cftTSS  =  SsumfirstleftTSS/countl; 

m  can  li  rstl  c  ftTS  S  n  to  J I H  i  gh  =  S  sum  (i  rs  tl  e  (\T  S  S  n  to  J I H  i  gh/co  un  t2 ; 

m  can  fi  rstl  cftTS  S  n  to  J I M  o  derate  =  Ssum  (i  rstlcftTSSntoJIModeratc/count3; 

meanfirstleftPL  =  SsumfirstleftPL/count4; 

meanfirstleftPLntoJIHigh  =  Ssum  firstleftP  LntoJ  1  High/count5 ; 

meanfirstleftPLntoJIModerate  =  SsumfirstleftPLntoJIModerate/count6; 

meantruetimedata  =  mean(sumkdata(:,l,:)); 

meandistuncert  =  mean(sumkdata(:,2,:)); 

meandistapart  =  mean(sumkdata(:,3,0); 

meanesttimedata  =  mean(sumkdata(:,6,:)); 

%  Calculate  the  sample  variance  of  the  estimated  time  to  impact 
for  p  =  1  :nt 
sumtimediff=0; 
timediff=0; 
for  q  =  1  :totalruns 

timediff  =  (sumkdata(q,6,p)-meanesttimedata(p))A2; 
sumtimediff=sumtimediff+timediff; 
end 

meansampletimesigma(p)  =  sqrt(sumtimediff  /(totalruns-1)); 
end 

figure(30); 

elf; 

plot(meantruetimedata(:)',meandistuncert(:)Vb',meantruetimedata(:)',meandistapart(:)','g’) 
legend('Distance  Uncertainty'/Distance  Apart'); 
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buf=sprintf('Estimated  End  Point'); 
title(buf) 

xlim([min(lowest)*DT  MAX  TIME- 1  *DT]) 
ylim(  [0  1000]) 
xlabel(’Simulated  Time  (s)') 
ylabel('Distance  (m)') 

figure(31); 

elf; 

plot(meantruetimedata(:)',abs((MAX_TIME-meantruetimedata(:)')- 
meanesttimedata(:)'),'b',meantruetimedata(:)',meansampletimesigma,'r' ) 
legend('Estimated  Error', 'Standard  Deviation’); 
buf=sprintf('Error  between  Time  Estimate  and  Truth'); 
title(buf) 

xlim([min(lowest)*DT  MAX  TIME- 1  *DT]) 
ylim(  [0  100]) 

xlabel(’Simulated  Time  (s)') 
ylabel('Time  (s)') 

figure(32); 

elf; 

plot(meantruetimedata(:)',probability(:),’k’) 
legend('Probability  of  Impact'); 
buf=sprintf('Probability  of  Impact'); 
title(buf) 

xlim([min(lowest)*DT  MAX  TIME- 1  *DT]) 
ylim([0  1]) 

xlabel('Simulated  Time  (s)') 
ylabel('Probability  of  Impact') 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  file  runs  the  decentralised  tracker.  The  default  condition  is  to 
%  do  a  new  run  every  time.  There  is  also  the  option  to  use  previous 
%  observation  runs  so  that  different  filters  can  be  compared. 

globals; 

ginit; 

if  ~exist('NEW_RUN','var') 

NEW_RUN=1; 

end 

%  If  we  choose  to  use  previous  data, 

%  then  the  existence  of  various  variables  needs  to  be  checked 
if  ~NEW_RUN 

if  ~exist(’observations’,'var’)  |  ~exist('targets’,'var’)  |... 

~exist('platforms’,'var')  |  ~exist(’times','var') 
error('No  existing  Data  to  perform  simulation’); 
end 

[num_nodes,num_times]=size(observations); 

nt=length(times); 

if  (num_nodes  ~=  NUM_NODES)  |  (num_times  ~=  nt) 
error(’Observation  Record  is  of  incorrect  dimension’); 
end 

if  nt~=  length(targets) 
error(’Target  set  is  of  incorrect  dimension’); 
end 

[num_times,num_plat]=size(platforms); 
if  (num_nodes  ~=  num_plat)  |  (numtimcs  ~=  nt) 
error(’Platform  set  is  of  incorrect  dimension’); 
end 

buf=sprintf(’01d  Run  Commencing  with  %d  nodes  and  %d 
steps\n’,num_nodes,num_times); 

disp(buf); 

end 

%  If  we  are  doing  a  new  run  then  simulate  the  target  and  platform  motions 
ifNEW  RUN 


time 
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clear  all; 

globals; 

ginit; 

NEW_RUN=1; 
times=0:DT  :MAX_TIME; 
num_times=length(times) ; 

buf=sprintf(’New  Run  Commencing  with  %d  nodes  and  %d 
steps\n',NUM_NODES,num_times-l); 
disp(buf); 

%  This  code  currently  apply  to  only  one  target 
%  One  target  considerably  simplifies  computation 
buf=sprintf('Generating  Target  Set\n');  disp(buf); 
targets=generate _ targets(times); 

%  Simulate  platforms  motion,  assuming  they  are  known 
buf=sprintf('Generating  Platform  Trajectories\n');  disp(buf); 
platforms=generate _ platforms(times); 

%  Generate  all  target's  attack  destinations 

destinations=generate _ destinations(times); 

else 

%  If  we  are  using  previous  data  set 

%  then  there  is  no  need  to  simulate  targets  or  platforms 

ginit; 

clear  Nodes; 
end 

%  In  every  case,  the  sensors  which  sit  on  the  platforms  are  defined 
%  Also,  this  allows  the  filters  to  be  changed  between  runs 
buf=sprintf('Defining  SensorsW);  disp(buf); 
filter=get _filter_params; 

for  i=l  :NUM_NODES 

Nodes(i)=make _ sensor(i, filter); 

end 

%  Establish  initial  topology 
com_net=init_net(platforms, Nodes); 

%  Establish  the  time  base 
ntimes=length(times); 

%  Define  space  to  record  results 
Results_y=zeros(ntimes,NUM_NODES,  filter  .XDIM); 


time 
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Resuks_Y=zeros(ntimes,NUM_NODES, filter  .XDIM, filter  .XDIM); 


% - 

% 

%  START  OF  FILTER  LOOPS  HERE 

% 

% - 


%  Run  loop  for  all  sensors  at  each  time. 

%  This  is  as  close  as  we  can  get  to  running  this  in  parallel 

for  n=l:ntimes 
kdata(l,n)=0; 
kdata(2,n)=0; 
kdata(3,n)=0; 
kdata(4,n)=0; 
kdata(5,n)=0; 
kdata(6,n)=0; 
kdata(7,n)=99999; 
kdata(8,n)  =  0; 
kdata(9,n)  =  0; 
kdata(10,n)  =  0; 
kdata(l  l,n)  =  0; 
leftT  S  S  (n)=N  aN ; 
leftTSSntoJIHigh(n)=NaN; 
leftTSSntoJIModerate(n)=NaN; 
leftPL(n)=NaN; 
leftPLnto  JIHigh(n)=NaN ; 
leftPLnto  JIModerate(n)=NaN ; 
end 

distapart  =  0; 
distuncert  =  0; 
disttoreach  =  0; 
speedtoreach  =  0; 
timetoreach  =  0; 

for  i=T:ntimes 
startplot=99999; 

buf=sprintf('Step:  time=%d',i-l);  disp(buf); 

%  Time  in  this  loop 
t=times(i); 

%  Generate  an  observation  for  each  node  at  this  time 
for  s=l:NUM  NODES 
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if  NEW_RUN  %  new  observation  simulation  required 

obs(s).report=sensor _ report(Nodes(s),  platforms,  targets,  s,i); 

observations^, i)=obs(s);  %  record  observations  for  plotting 
else  %  use  old  observation  data 
obs(s)=observations(s,i); 
end 
end 

%  First,  do  a  local  prediction 
for  s=l  :NUM_NODES 
Nodes(s)=local_predict(Nodes(s),t); 
end 

%  Second,  a  local  update  generating  y  tilde  at  each  site 
for  s=l  :NUM_NODES 
Nodes(s)=local_update(Nodes(s),obs(s),s); 
end 

%  Next,  establish  network  connection 

com_net=set_net_topology(com_net, Nodes, t, 'BROADCAST'); 

%  Then  communicate 

%  This  sets  up  and  exchanges  data  in  channel  filters 
for  s=l  :NUM_NODES 
Nodes(s)=communicate(Nodes,com_net,s); 
end 

%  Finally  do  global  update  by  adding 
for  s=l  :NUM_NODES 
Nodes(s)=assimilate(Nodes(s),t); 
end 

%  Record  results 
for  s=l  :NUM_NODES 

Results_y(i,s,:)=Nodes(s).est.y; 

Results_Y(i,s,:,:)=Nodes(s).est.Y; 

PredictedResults_y(i,s,:)=Nodes(s).pred.y; 

PredictedResults_Y(i,s,:,:)=Nodes(s).pred.Y; 

end 

s=l; 

warning  off; 

P=inv(squeeze(Results_Y(i,s, :,:)));  %choose  any  sensor  since  all  of  them  have  the 
same  track  information 
if  P  ~=  inf 
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%  Calculate  the  position  uncertainty  of  the  target 

%  at  present  location 

x=P*squeeze(Results_y(i,s,:)); 

sigma=sqrt(P); 

xuncert  =  sigma(l,l); 

yuncert  =  sigma(3,3); 

distuncert  =  sqrt(xuncertA2  +  yuncertA2); 

b  =  circle!  [x(l),  x(3)], distuncert); 

%  Project  the  position  uncertainty  of  the  target 

%  at  final  destination 

FF=[1  (ntimes-i)*DT  0  0; 


0  1 
0  0 
0  0 


0  0; 

1  (ntimes-i)*DT; 

0  1]; 


GG=[1  0  0  0; 

0  100; 

00  10; 

00  0  1]; 

QQ=  [((ntimes-i) *DT)A2 * SIGM A_XDOTA2  0  0  0; 

0  SIGMA_XDOTA2  0  0; 

0  0  ((ntimes-i)*DT)A2*SIGMA_YDOTA2  0; 

0  0  0  SIGMA_YDOTA2]; 

MM=(inv(FF))'*squeeze(Results_Y(i,s,:,:))*(inv(FF)); 

SSigma=GG'*MM*GG+inv(QQ); 

OOmega=MM  *  GG  *  inv(S  Sigma); 

projYpred=MM-(OOmega*SSigma*OOmega'); 

proj  Ypred=force_sym(proj  Ypred); 

projP  =  inv(proj  Ypred); 

projPsigma  =  sqrt(projP); 

projxuncert  =  projPsigma(  1,1); 

projyuncert  =  projPsigma(3,3); 

proj  distuncert  =  sqrt(projxuncertA2  +  projyuncertA2); 

projypred=(eye(4,4)-(OOmega*GG'))*(inv(FF))'*squeeze(Results_y(i,s,:)); 

proj  state  =  proj  P*proj  ypred; 

%  Check  if  SAW  has  entered  Port  Limits 
if  min(inpolygon  (b.X,  b.Y,  TSS_X,  TSS_Y))==  0  &&... 
min(inpolygon  (b.X,  b.Y,  PL_X,  PL_Y))  ==  0 

buf=sprintf(' Warning:  Entered  Port  Limits  at  Timestep=%d',i-1);  disp(buf); 
%  Check  if  SAW  is  turning  towards  Jurong  Island 
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disttoreach  =  sqrt(  abs(x(l)-302.1 144444)A2+... 

abs(x(3)-347.7716667)A2); 
speedtoreach  =  sqrt(x(2)A2+x(4)A2); 
timetoreach  =  disttoreach/speedtoreach; 

%  Calculate  the  time  target  left  Port  Limits 
leftPL(i)  =  MAXTIME-timetoreach; 

%  Calculate  the  distance  between  the  projected  target  end  point 
%  and  Jurong  Island 

distapart  =  sqrt((projstate(l)  -  302.1 144444)A2+(projstate(3)  -  347.77 1 6667)A2); 

%  Threat  level  assessment 

%  Send  warning  if  target  is  within  0.5nm  to  Jurong  Island 
if  distapart  <  5  &&  MAX  TIME  -  (i-l)*DT  >=  0 
buf=sprintf(' Warning:  Heading  Towards  Jurong  Island.  Time  to  Impact  =  %.2f 
sec’, timetoreach);  disp(buf); 

if  projdistuncert  >  distapart 

buf=sprintf('Waming:  High  Confidence  of  Impacf);disp(buf); 
startplot=i-l; 

leftPLntoJIHigh(i)  =  MAXTIME-timetoreach; 

else 

buf=sprintf(' 'Warning:  Moderate  Confidence  of  Impact’ );disp(buf); 
startplot=i- 1 ; 

leftPLntoJIModerate(i)  =  MAXTIME-timetoreach; 
end 
end 

%  Check  if  SAW  has  left  Traffic  Separation  Scheme 
elseif  min(inpolygon  (b.X,  b.Y,  TSS_X,  TSS_Y))==  0  &&... 
min(inpolygon  (b.X,  b.Y,  PL_X,  PL_Y))  ==  1 
buf=sprintf( Warning:  Left  Traffic  Separation  Scheme  at  Timestep=%d',(i-1)); 
disp(buf); 

%  check  if  SAW  is  turning  towards  Jurong  Island 
disttoreach  =  sqrt(  abs(x(l)-302.1 144444)A2+... 

abs(x(3)-347.7716667)A2); 
speedtoreach  =  sqrt(x(2)A2+x(4)A2); 
timetoreach  =  disttoreach/speedtoreach; 

%  Calculate  the  time  target  left  TSS 
leftTSS(i)  =  MAXTIME-timetoreach; 

%  Calculate  the  distance  between  the  projected  target 

%  end  pointand  Jurong  Island 

distapart  =  sqrt((projstate(l)  -  302.1 144444) A2  +... 

(projstate(3)  -  347.7716667)A2); 
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%  Threat  level  assessment 

%  Send  warning  if  target  is  within  0.5nm  to  Jurong  Island 
if  distapart  <  5  &&  MAX  TIME  -  (i-l)*DT  >=  0  ° 
buf=sprintf('Warning:  Heading  Towards  Jurong  Island.  Time  to  Impact  =  %.2f 
sec',timetoreach);  disp(buf); 

if  projdistuncert  >  distapart 

buf=sprintf(’ Warning:  High  Confidence  of  Impacf);disp(buf); 
startplot=i-l; 

leftTSSntoJIHigh(i)  =  MAXTIME-timetoreach; 

else 

buf=sprintf(’ Warning:  Moderate  Confidence  of  Impact');disp(buf); 
startplot=i- 1 ; 

leftTSSntoJIModerate(i)  =  MAXTIME-timetoreach; 
end 
end 
end 

kdata(  1  ,i)=times(i); 

kdata(2,i)=projdistuncert*  1 80; 

kdata(3,i)=distapart*  1 80; 
kdata(4,i)=disttoreach*  1 80; 
kdata(5,i)=speedtoreach*  180; 
kdata(6,i)=timetoreach; 
kdata(7,i)=startplot; 
kdata(8,i)=projP(  1,1); 
kdata(9,i)=projP(3,3); 
kdata(  1 0,i)=proj  state(  1 ); 
kdata(l  l,i)=projstate(3); 

end 

end 

disp('Plotting  Data...'); 

figure(20); 

elf; 

plot(kdata(  1 ,  :),(kdata(2,  :)),'b’,kdata(  1  ,:),kdata(3 ,  :),'g’) 
legend('Distance  Uncertainty'/Distance  Apart'); 
buf=sprintf('Estimated  End  Point'); 
title(buf) 

xlim( [min(kdata(7, 1 000:MAX_TIME/DT))*DT  MAX  TIME- 1  *DT]) 
axis  'auto  y' 

xlabel(’Simulated  Time  (s)') 
ylabel('Distance  (m)') 
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figure(21); 

elf; 

plot(kdata(l,:),kdata(4,:),'b',kdata(l,:),kdata(5,:),'g’,kdata(l,:),kdata(6,:),’r') 
legend('Distance  to  Impact’,'Speed  to  Impact',  ’Time  to  Impact’); 
buf=sprintf('Projected  Distance,  Speed  and  Time  to  Impact’); 
title(buf) 

xlim( [min(kdata(7, 1 000:MAX_TIME/DT))*DT  MAX  TIME- 1  *DT]) 
axis  'auto  y' 

xlabel(’Simulated  Time  (s)’) 


%  Record  the  times  the  target  crossed  TSS,  PL,  &  corresponding  threat  level 
firstleftTSS  =  min(leftTSS(1000:MAX_TIME/DT)); 

TirstleftTSSntoJIHigh  =  min(leftTSSntoJIHigh(1000:MAX_TIME/DT)); 
firstleftTSSntoJIModerate  =  min(leftTSSntoJIModerate(1000:MAX_TIME/DT)); 
firstleftPL  =  min(leftPL(  1 000  :M  AX_TIME/DT)); 
firstleftPLntoJIHigh  =  min(leftPLntoJIHigh(1000:MAX_TIME/DT)); 
firstleftPLnto  JIModerate  =  min(leftPLnto  JIModerate(  1 000  :MAX_TIME/DT)); 

time  =  0; 
for  i=l:ntimes 

P=inv(squeeze(Results_Y(i,s, :,:))); 
if  P  ==  inf 
time  =  i; 
end 
end 

warning  on; 

post _ sim(targets,  observations,  platforms,  destinations); 

plot _ tracks(targets,Results_y,Results_Y, times) 

plot _ errors(targets,Results_y,Results_Y,PredictedResults_y, . . . 

PredictedResults_Y,  times,  1 ,2, time) 

plot _ coms(platforms,com_net) 

%  This  file  defines  the  global  variables 


%  General  information 


global  TSS  X; 
global  TSS  Y; 
global  PL_X; 
global  PL  Y ; 
global  DT; 


%  Traffic  Separation  Scheme  x-coordinates 
%  Traffic  Separation  Scheme  y-coordinates 
%  Port  Limits  x-coordinates 
%  Port  Limits  y-coordinates 
%  simulation  time-step 


global  MAX  TIME;  %  maximum  simulation  time 


%  Target  definitions 

global  TARGET  TYPE;  %  defines  type  of  target  (xy  or  V  phi) 

global  NUM  TARGETS;  %  number  of  targets  to  be  simulated 
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%  Platform  definitions 

global  PLATFORM;  %  Platform  data  [xO,  yO,  phiO,  vel] 
global  NUMPLATFORMS;  %  number  of  platforms 
global  MAX_RANGE;  %  maximum  observable  range 
global  SIGMA  XDOT; 
global  SIGMA  YDOT; 

%  Destination  definitions 

global  DESTINATION;  %  Destination  data  [xO,  yO,  phiO,  vel] 
global  NUM_DESTINATIONS;%  number  of  destinations 

%  Communications  definitions 
global  NUM_NODES;  %  number  of  sensor  nodes 

global  NUM_CHANS;  %  number  of  channels  per  node 
global  FULL_CON;  %  connectivity 

%  State  and  Sensor  definitions 

global  SIGMA  Q;  %  process  noise  standard  deviation  for  feature  model 

global  SIGMA_RANGE_MPA;  %  Range  observation  noise 

global  SIGMA  BEARING  MPA;  %  Bearing  observation  noise 

global  SIGMA_RANGE_MPAC;  %  Range  observation  noise 

global  SIGMA  BEARING  MPAC;  %  Bearing  observation  noise 

global  SIGMA  RANGE  PV;  %  Range  observation  noise 

global  SIGMA  BEARING  PV;  %  Bearing  observation  noise 

global  SIGMA  RANGE  PCG;  %  Range  observation  noise 

global  SIGMA  BEARING  PCG;  %  Bearing  observation  noise 

global  Rfilter_MPA;  %  observation  noise  covariance  for  MPA 

global  Rfilter  MPAC;  %  observation  noise  covariance  for  MPAC 

global  Rfilter  PV;  %  observation  noise  covariance  for  PV 

global  Rfilter  PCG;  %  observation  noise  covariance  for  PCG 

global  F;  %  state  transition  equation 

global  XSIZE;  %  state  dimension 

global  ZSIZE;  %  observation  dimension 

%  Results  "database" 

global  kdata; 

global  esttimesigma; 

global  startplot; 

global  firstleftTSS; 

global  firstleftTSSntoJIHigh; 

global  firstleftTSSntoJIModerate; 

global  firstleftPL; 

global  firstleftPLntoJIHigh; 

global  firstleftPLntoJIModerate; 
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%  This  file  initializes  the  global  variables 


%  General  polygon  for  Traffic  Separation  Scheme  (TSS) 

X  =  [738.9629;  700.8405;  503.0354;  343.3085;  301.0754;  188.7756;  142.3798; 
291.7565;  356.6971;  474.1785;  703.9221;  738.9538;  738.9629]; 

Y  =  [443.8223;  412.1040;  377.3690;  303.7337;  330.3691;  359.0929;  316.1193; 
230.0498;  289.3964;  351.7903;  379.3511;  408.0001;  443.8223]; 

TSSX  =  X; 

TSSY  =  Y; 


%  General  polygon  for  Port  Limits,  estimated  0.5  mile  beyond  the  TSS 
[PL_X,  PL_Y]  =  bufferm2('xy’,X,Y,5,'ouf); 

MAX_TIME=1 1500;  %  simulation  duration 

DT=10;  %  simulation  time-step 

%  Target  definitions 

NUM_TARGETS=1 ;  %  number  of  targets  to  be  tracked 


%  Platform  definitions 
NUM_PLATFORMS=9; 


%  Destination  definitions 
NUM_DESTINATIONS=T; 


%  Number  of  sensor  nodes 
NUMN  ODE  S=NUM_PL  ATF  ORMS ; 

NUM_CHANS=9;  %  number  of  channels  caters  up  to  9  nodes 

FULL_CON=l;  %  set  to  full  connectivity  between  all  nodes 


%  State  and  Sensor  definitions 

SIGMA_RANGE_MPA=5;  %  0.5nm  range  error  equivalent 

SIGMA_BEARING_MPA=0.0525;  %  3  degrees  bearing  error  in  radians 
SIGMA_RANGE_MPAC=6;  %  0.6nm  range  error  equivalent 

SIGMA_BEARING_MPAC=0.0875;  %  5  degrees  bearing  error  in  radians 
SIGMA_RANGE_PV=3;  %  0.3nm  range  error  equivalent 

SIGMA_BEARING_PV=0.0525;  %  3  degrees  bearing  error  in  radians 
SIGMA_RANGE_PCG=4;  %  0.4nm  range  error  equivalent 

SIGMA_BEARING_PCG=0.0875;  %  5  degrees  bearing  error  in  radians 
Rfilter  MPA  =  [ SIGM A  RAN GE  MP AA2  0;0  SIGMA  RANGE  MP AA2] ; 
RfilterMPAC  =  [  S IGM  ARAN  GEMP  AC  A2  0;0  SIGMA_RANGE_MPACA2] ; 
Rfilter  PV  =  [ S IGM A_RAN GE_P VA2  0;0  SIGMA  RANGE  P VA2] ; 

Rfilter  PCG  =  [SIGMA_RANGE_PCGA2  0;0  SIGMA_RANGE_PCGA2]; 

XSIZE=4;  %  number  of  state  components 

ZSIZE=2;  %  number  of  sensor  measurement  components 
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SIGMA_XDOT=0. 00286;  %  std  dev  of  lknot  in  the  x-direction 

SIGMA_YDOT=0. 00286;  %  std  dev  of  lknot  in  the  y-direction 

%  Results  "database" 

kdata  =  [7  MAXTIME/DT+l]; 

function  [latb,lonb]  =  bufferm2(varargin)  %lat, Ion, dist, direction, npts,outputformat) 
%BUFFERM2  Computes  buffer  zone  around  a  polygon 

% 

%  [latb,lonb]  =  bufferm2(lat, Ion, dist, direction) 

%  [latb,lonb]  =  bufferm2(lat, Ion, dist, direction, npts) 

%  [latbjonb]  =  bufferm2(lat, Ion, dist, direction, npts, outputformat) 

%  [xb,  yb]  =  bufferm2('xy',x,y, dist, direction, npts, outputformat) 

% 

%  This  function  was  originally  designed  as  a  replacement  for  the  Mapping 
%  Toolbox  function  bufferm,  which  calculates  a  buffer  zone  around  a 
%  polygon.  The  original  bufferm  function  had  some  serious  bugs  that  could 
%  result  in  incorrect  buffer  results  and/or  errors,  and  was  also  very  slow. 

%  As  of  R2006b,  those  bugs  have  been  fixed.  However,  this  version  still 
%  maintains  a  few  advantages  over  the  original: 

% 

%  -  Can  be  applied  to  polygons  in  either  geographical  space  (as  in 
%  bufferm)  or  in  cartesian  coordinates. 

% 

%  -  Better  treatment  of  polygon  holes.  The  original  function  simply 
%  filled  in  all  holes;  this  version  trims  or  pads  holes  according  to  the 
%  buffer  width  given. 

% 

%  Input  and  output  format  is  identical  to  bufferm  unless  the  'xy'  option  is 
%  specified,  so  it  can  be  used  interchangeably. 

% 

%  Input  variables: 

% 

%  lat:  Latitude  values  defining  the  polygon  to  be  buffered. 

%  This  can  be  either  a  NaN-delimited  vector,  or  a  cell 

%  array  containing  individual  polygonal  contours  (each  of 

%  which  is  a  vector).  External  contours  should  be  listed 

%  in  a  clockwise  direction,  and  internal  contours  (holes) 

%  in  a  counterclockwise  direction. 

% 

%  Ion:  Longitude  values  defining  the  polygon  to  be  buffered. 

%  Same  format  as  lat. 

% 

%  dist:  Width  of  buffer,  in  degrees  of  arc  along  the  surface 

%  (unless  'xy'  is  used,  in  which  case  units  correspond  to 

%  x-y  coordinates) 
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% 

%  direction:  'in'  or  'out' 

% 

%  npts:  Number  of  points  used  to  contruct  the  circles  around 

%  each  polygon  vertex.  If  omitted,  default  is  13. 

% 

%  outputformat:  'vector'  (NaN-delimited  vectors),  'cutvector' 

%  (NaN-clipped  vectors  with  cuts  connecting  holes  to  the 

%  exterior  of  the  polygon),  or  'cell'  (cell  arrays  in 

%  which  each  element  of  the  cell  array  is  a  separate 

%  polygon),  defining  format  of  output.  If  omitted, 

%  default  is  'vector'. 

% 

%  'xy':  If  first  input  is  'xy',  then  data  will  be  assumed  to 

%  lie  on  a  cartesian  plane  rather  than  on  a  sphere.  Use 

%  x  and  y  coordinates  as  first  two  inputs  rather  than  lat 

%  and  Ion.  Units  of  x,  y,  and  distance  should  be  the 

%  same. 

% 

%  Output  variables: 

% 

%  latb:  Latitude  values  for  buffer  polygon 

% 

%  lonb:  Longitude  values  for  buffer  polygon 

% 

%  Example: 

% 

%  load  conus 

%  tol  =  0. 1 ;  %  Tolerance  for  simplifying  polygon  outlines 
%  [reducedlat,  reducedlon]  =  reducem(gtlakelat,  gtlakelon,  tol); 

%  dist  =  1 ;  %  Buffer  distance  in  degrees 
%  [latb,  lonb]  =  bufferm2(reducedlat,  reducedlon,  dist,  'out'); 

%  figure('Renderer', ’painters') 

%  usamap({’MN’,'NY’}) 

%  geoshow(latb,  lonb,  'DisplayType',  'polygon',  'FaceColor',  'yellow') 
%  geoshow(gtlakelat,  gtlakelon,... 

%  'DisplayType',  'polygon',  'FaceColor',  'blue') 

%  geoshow(uslat,  uslon) 

%  geoshow(statelat,  statelon) 

% 

%  See  also: 

% 

%  buffenn,  polybool 
%  Copyright  2010  Kelly  Kearney 
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% - 

%  Check  input 
% - 


error(nargchk(3,7,nargin)); 

%  Detennine  if  geographic  or  cartesian 

if  ischar(varargin{l})  &&  strcmp(varargin{l},  'xy') 
geo  =  false; 

param  =  varargin(2:end); 
else 

geo  =  true; 
param  =  varargin; 
end 

%  Set  defaults  if  not  provided  as  input 
nparam  =  length(param); 
if  geo 

[lat,  Ion,  dist]  =  deal(param{l:3}); 
else 

[Ion,  lat,  dist]  =  deal(param{l:3});  %  Ion  =  x,  lat  =  y  for  mental  clarity,  will  switch 
back  at  end 
end 

if  nparam  <  4 
direction  =  'out'; 
else 

direction  =  param  {4}; 
end 

if  nparam  <  5 
npts  =  13; 
else 

npts  =  param{5}; 
end 

if  nparam  <  6 

outputformat  =  'vector'; 
else 

outputformat  =  param  {6}; 
end 
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%  Check  format  and  dimensions  of  input 

if  ~ismember(direction,  {'in',  'out'}) 

error('Direction  must  be  either  "in"  or  "out".'); 
end 

if  ~ismember(outputfonnat,  {'vector',  'cutvector',  'cell'}) 
error('Unrecognized  output  format  flag.'); 
end 

if  ~isnumeric(dist)  ||  numel(dist)  >  1 
error('Distance  must  be  a  scalar.’) 
end 

if  ~isnumeric(npts)  ||  numel(npts)  >  1 

error('Number  of  points  must  be  a  scalar.') 
end 

if  iscell(lat) 

for  il  =  1  :numel(lat) 

if  ~isvector(lat{il})  ||  ~isvector(lon{il})  ||  ~isequal(length(lat{il}),  length(lon{il})) 
error('Lat  (or  x)  and  Ion  (or  y)  must  be  vectors  or  cells  of  vectors  with  identical 
dimensions’); 
end 

lat{il}  =  lat{il}(:); 
lon{il}  =  lon{il}(:); 
end 
else 

if -isvector(lat)  ||  -isvector(lon)  ||  ~isequal(length(lat),  length(lon)) 

error('Lat  (or  x)  and  Ion  (or  y)  must  be  vectors  or  cells  of  vectors  with  identical 
dimensions’); 
end 

lat  =  lat(:); 

Ion  =  lon(:); 
end 


%■ 


%  Split  polygon(s)  into 
%  separate  faces 


%■ 


if  iscell(lat) 

[lat,  Ion]  =  polyjoin(lat,  Ion);  %  In  case  multiple  faces  in  one  cell, 
end 
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[latcells,  loncells]  =  polysplit(lat,  Ion); 


% - 

%  Create  buffer  shapes 
% - 


plotflag  =  0; 

if  plotflag 

Plt.x  =  Ion; 

Plt.y  =  lat; 

end 

latcrall  =  cell(0); 
loncrall  =  cell(0); 

for  ipoly  =  1  :length(latcells) 

%  Circles  around  each  vertex 

if  geo 

[late,  lone]  =  calccircgeo(latcells{  ipoly},  loncells  {ipoly},  dist,  npts); 
else 

[lone,  late]  =  calccirccart(loncells{  ipoly},  latcells  {ipoly},  dist,  npts); 
end 

%  Rectangles  around  each  edge 
if  geo 

[latr,  lonr]  =  calcrecgeo(latcells  {ipoly},  loncells  {ipoly},  dist); 
else 

[lonr,  latr]  =  calcreccart(loncells  {ipoly},  latcells  {ipoly},  dist); 
end 

%  Union  of  circles  and  rectangles 

if  plotflag 

Plt.rectx  =  lonr; 

Plt.recty  =  latr; 

Plt.circx  =  lone; 

Plt.circy  =  late; 
end 
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[late,  lone]  =  multipolyunion(latc,  lone); 

[latr,  lonr]  =  multipolyunion(latr,  lonr); 

if  plotflag 

Plt.rectcombox  =  lonr; 

Plt.rectcomboy  =  latr; 

Plt.circcombox  =  lone; 

Plt.circcomboy  =  late; 
end 

[loner,  later]  =  polybool(’+',  lonr,  latr,  lone,  late); 

%  Union  of  new  circle/rectangle  combo  with  that  from  other  faces 

[loncrall,  latcrall]  =  polybool('+',  loncrall,  latcrall,  loner,  later); 

%  Plotting  (for  debugging  only) 

if  plotflag 

Plt.allx  =  loncrall; 

Pit. ally  =  latcrall; 

if  ipoly  ==  1 
figure; 

plot(Plt.x,  Plt.y,  'k',  'linewidth',  2); 
hold  on 
end 

plot(cat(2,  Plt.rectx]:}),  cat(2,  Plt.recty { : }),  ’b'); 
plot(cat(2,  Plt.circx]:}),  cat(2,  Plt.circy { : }),  'r'); 
plot(Plt.allx{l},  Plt.ally { 1 } ,  'g',  ’linewidth',  2); 

end 


% - 

%  Calculate  union/difference 
% - 


switch  direction 
case  'out' 

[lonb,  latb]  =  polybool('+',  loncells,  latcells,  loncrall,  latcrall); 
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case  'in’ 

[lonb,  latb]  =  polybool(’-',  loncells,  latcells,  loncrall,  latcrall); 
end 

if  plotflag 

[Plt.yfinal,  Plt.xfinal]  =  polyjoin(latb,  lonb); 

plot(Plt.xfinal,  Plt.yfinal,  'linestyle', 'color',  [0  .5  0],  'linewidth',  2); 
end 


% - 

%  Reformat  output 
% - 


if  -geo 

y  =  latb;  %  Switch,  since  cartesion  uses  opposite  order 
x  =  lonb; 
latb  =  x; 
lonb  =  y; 
end 

switch  outputformat 
case  'vector' 

[latb,  lonb]  =  polyjoin(latb,  lonb); 
case  'cutvector' 

[latb,  lonb]  =  polycut(latb,  lonb); 
case  'cell' 
end 


**** 

function  [late,  lone]  =  calccircgeo(lat,  Ion,  radius,  npts) 
%  lat  and  Ion:  n  x  1  vectors 
%  radius:  scalar 

radius  =  ones(length(lat),  1 )  *  radius; 

[late,  lone]  =  scirclel(lat,  Ion,  radius,  [],[],[],  npts); 
late  =  num2cell(latc,  1); 
lone  =  num2cell(lonc,  1); 

function  [latr,  lonr]  =  calcrecgeo(lat,  Ion,  halfwidth) 

%  lat  and  Ion:  n  x  1  vectors 
%  halfwidth:  scalar 
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range  =  halfwidth  *  ones(length(lat)-l,  1); 

az  =  azimuth(lat(l:end-l),  lon(l:end-l),  lat(2:end),  lon(2:end)); 

[latbll,lonbll]  =  reckon(lat(l:end-l),  lon(l:end-l),  range,  az-90); 
[latbrl,lonbrl]  =  reckon(lat(l:end-l),  lon(l:end-l),  range,  az+90); 
[latbl2,lonbl2]  =  reckon(lat(2:end),  lon(2:end),  range,  az-90); 
[latbr2,lonbr2]  =  reckon(lat(2:end),  lon(2:end),  range,  az+90); 

latr  =  [latbll  latbl2  latbr2  latbrl  latbl  1  ]'; 
lonr=  [lonbll  lonbl2  lonbr2  lonbrl  lonbll]’; 
latr  =  num2cell(latr,  1); 
lonr  =  num2cell(lonr,  1); 

function  [latu,  lonu]  =  multipolyunion(lat,  Ion) 

%  lat  and  Ion  are  n  x  1  cell  arrays  of  vectors 

latu  =  lat  { 1 } ; 
lonu  =  lon{  1 } ; 

for  ip  =  2:length(lat) 

[lonu,  latu]  =  polybool('+',  lonu,  latu,  Ion  {ip},  lat  {ip}); 
end 

[latu,  lonu]  =  polysplit(latu,  lonu); 


function  [xc,  yc]  =  calccirccart(x,  y,  radius,  npts) 

ang  =  linspace(0,  2*pi,  npts+1); 

ang  =  ang(end-l:-l:l); 

xc  =  bsxfun(@plus,  x,  radius  *  cos(ang)); 

yc  =  bsxfun(@plus,  y,  radius  *  sin(ang)); 

xc  =  num2cell(xc',  1); 

yc  =  num2cell(yc',  1); 

%  if  ~ispolycw(x,y) 

%  [xc,yc]  =  poly2ccw(xc,yc); 

%  end 

function  [xrec,  yrec]  =  calcreccart(x,  y,  halfwidth) 

dx  =  diff(x); 
dy  =  diff(y); 

isl  =  dx  >=  0  &  dy  >=  0; 
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is2  =  dx  <  0  &  dy  >=  0; 
is3  =  dx  <  0  &  dy  <  0; 
is4  =  dx  >=  0  &  dy  <  0; 


ishl  =  dy  ==  0  &  dx  >  0; 
ish2  =  dy  ==  0  &  dx  <  0; 


theta  =  zeros(5,l); 

theta(isl  |  is3)  =  atan(dy(isl  |  is3)./dx(isl  |  is3)); 
theta(is2  |  is4)  =  -atan(dy(is2  |  is4)./dx(is2  |  is4)); 

[xl,xr,yl,yr]  =  deal(zeros(size(dx))); 

xl(isl)  =  -halfwidth  *  sin(theta(isl)); 
xr(isl)  =  halfwidth  *  sin(theta(isl)); 
yl(isl)  =  halfwidth  *  cos(theta(isl)); 
yr(isl)  =  -halfwidth  *  cos(theta(isl)); 

xl(is2)  =  -halfwidth  *  sin(theta(is2)); 
xr(is2)  =  halfwidth  *  sin(theta(is2)); 
yl(is2)  =  -halfwidth  *  cos(theta(is2)); 
yr(is2)  =  halfwidth  *  cos(theta(is2)); 

xl(is3)  =  halfwidth  *  sin(theta(is3)); 
xr(is3)  =  -halfwidth  *  sin(theta(is3)); 
yl(is3)  =  -halfwidth  *  cos(theta(is3)); 
yr(is3)  =  halfwidth  *  cos(theta(is3)); 

xl(is4)  =  halfwidth  *  sin(theta(is4)); 
xr(is4)  =  -halfwidth  *  sin(theta(is4)); 
yl(is4)  =  halfwidth  *  cos(theta(is4)); 
yr(is4)  =  -halfwidth  *  cos(theta(is4)); 

xrec  =  [xl+x(l:end-l)  xl+x(2:end)  xr+x(2:end)  xr+x(l:end-l)  xl+x(l:end-l)]; 
yrec  =  [yl+y(l:end-l)  yl+y(2:end)  yr+y(2:end)  yr+y(l:end-l)  yl+y(  1 :  end- 1 )] ; 

xrec  =  num2cell(xrec,  2); 
yrec  =  num2cell(yrec,  2); 
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%  This  function  draws  a  circle  for  a  given  center  and  radius 
function  bubble  =  circle(center, radius) 

Resolution  =  100; 

THETA=linspace(0,2*pi, Resolution); 

RHO=ones(l  ,Resolution)*radius; 

[X,Y]  =  pol2cart(THETA,RHO); 
bubble.X=X+center(l); 
bubble.  Y=Y+center(2); 
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%  This  function  generates  the  SAW's  intended  destinations 
%  (i.e.  Areas  Al,  A2,  and  A3  of  Jurong  Island) 

function  destinations  =  generate _ destinations(times) 

%  Define  the  global  variables 
globals; 

%  Define  the  destination  locations 
for  i=l  :NUM_DESTINATIONS 
%  Area  A 1 
if  i==l 

destinations(  1  ,i)  =  makedestination; 
destinations(  1  ,i).time=times(  1 ); 
destinations(  1  ,i)-id=i; 
destinations(l,i).x  =  302.1 144444; 
destinations(l,i)-y  =  347.7716667; 

%  Now  iterate  for  all  times 
[temp,ntimes]=size(times); 
for  n=2:ntimes 

destinations(n,i)=destination_step(destinations(n- 1  ,i),timcs(n)); 
end 
end 

%  Area  A2 
if  i==2 

destinations(  1  ,i)  =  makedestination; 
destinations(  1  ,i)  .time=times(  1 ); 
destinations(  1  ,i)-id=i; 
destinations(l,i).x  =  322.731 1  111; 
destinations(l,i)-y  =  367.21 16667; 

%  Now  iterate  for  all  times 
[temp  ,ntimes] =size(times) ; 
for  n=2:ntimes 

dcstinations(n,i)=dcstination_stcp(dcstinations(n-l  ,i),timcs(n)); 
end 
end 

%  Area  A3 
if  i==3 

destinations(  1  ,i)  =  makedestination; 
destinations(  1  ,i).timc=times(  1 ); 
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destinations(  1  ,i)-id=i; 

destinations(l,i)-x  =  343.3450000; 
destinations(l,i).y  =  382.5572222; 

%  Now  iterate  for  all  times 
[temp,ntimes]=size(times); 
for  n=2:ntimes 

destinations(n,i)=destination_step(destinations(n-l,i),times(n)); 

end 

end 

end 
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%  This  function  initialises  a  new  destination 


function  new=make_destination 

new=struct('id',0, 'time', 0,'x',0,'y',0, 'phi', 0,'vel',0, ’gamma’, 0); 
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%  This  function  is  to  step  a  destination 

function  next_destination=destination_step(destination,time) 
globals; 

%  This  is  a  no  motion  model 
dt=time-destination.time; 

nextdestination  =  makedestination;  %  space  for  destination  data  structure 

next_destination.id=destination.id; 

nextdestination.time  =  time; 

nextdestination.x  =  destination.x; 

nextdestination.y  =  destination.y; 

nextdestination.phi  =  destination.phi; 

nextdestination.vel  =  destination.vel; 

nextdestination.gamma  =  destination,  gamma; 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  generates  true  target  information  for  later  observation 
%  and  tracking.  Dynamics  of  target  are  contained  in  "target  step",  number 
%  targets  is  defined  by  the  globally  defined  variable  NUM_TARGETS.  A  data 
%  point  is  generated  at  each  of  a  set  of  "times.” 

function  targets  =  generate _ targets(times) 

%  Define  the  global  variables 
globals; 

%  Define  a  SAW  at  pre-fixed  location  in  the  TSS 
for  i=l 

targets(l,i)  =  make_target; 
targets(  1  ,i).time=times(  1 ); 
targets(l,i).id=i; 
targets(l,i).x  =  738.9616667; 
targets(l,i).y  =  438.7050000; 
targets(  1 ,  i) .  gamma=0 ; 

%  Now  iterate  for  all  times 
[temp,ntimes]=size(times); 
for  n=2:ntimes 

targets(n,i)=target_step_SAW_path(targets(n- 1  ,i),times(n)); 
end 
end 

%  Create  non-SAW  contacts  at  random  locations  within  the  TSS 
%  Not  used  since  this  thesis  covers  a  single  target 
for  i=2:NUM_TARGETS 
targets(l,i)  =  make_target; 
targets(  1  ,i).time=times(  1 ); 
targets(l,i).id=i; 

x  =  TSS_X(l)*rand; 
y  =  TSS_Y(l)*rand; 

while  (inpolygon  (x,  y,TSS_X,  TSS_Y)  ~=  1) 
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x  =  TSS_X(l)*rand; 
y  =  TSS_Y(l)*rand; 
end 

targets(l,i)-x  =  x; 
targets(l,i).y  =  y; 
targets(l,i)-phi  =  2*pi*rand  -pi; 

targets(l,i).vel  =  MINTARGETVEL  +  (M  AX_T  ARGETVEL- 

MIN_TARGET_VEL)*rand; 
targets(l  ,i)-gamma=0; 

%  Now  iterate  for  all  time 
[temp,ntimes]=size(times); 
for  n=2:ntimes 

targets(n,i)=target_step(targets(n- 1  ,i),timcs(n)); 
end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  This  function  initialises  a  new  target 

function  new=make_target 

new=struct('id',0, 'time', 0,'x',0,'yC0, 'phi', 0,'vel',0, 'gamma', 0); 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  is  the  trajectory  taken  by  the  SAW 

function  next_target=target_step_SAW_path(target,time) 

globals; 

dt=time-target.time; 
nexttarget  =  maketarget; 
next_target.id=target.id; 
nexttarget.time  =  time; 

%  target.vel  =  0.042870370  (in  m/s  which  equates  to  15knots) 

%  target.vel  =  0.059160494  (in  m/s  which  equates  to  20knots) 

%  target.vel  =  0.060018519  (in  m/s  which  equates  to  21  knots) 

%  target.vel  =  0.062876543  (in  m/s  which  equates  to  22knots) 

if  (next  target.time  >=  0  &&  next  target.time  <  1086) 
next_target.x  =  target.x  +  dt*0.042870370*cos(-pi+0.720); 
next_target.y  =  target. y  +  dt*0.042870370*sin(-pi+0.720); 
next_target.phi  =  -pi+0.720; 
next_target.vel  =  0.042870370; 

elseif  (next_target.time  >=  1086  &&  next_target.time  <  5890) 
next_target.x  =  target.x  +  dt*0.042870370*cos(-pi+0.170); 
next_target.y  =  target.y  +  dt*0.042870370*sin(-pi+0.170); 
next_target.phi  =  -pi+0.170; 
next_target.vel  =  0.042870370; 

elseif  (next  target.time  >=  5890  &&  next  target.time  <  10199) 
next_target.x  =  target.x  +  dt*0.042870370*cos(-pi+0.430); 
next_target.y  =  target.y  +  dt*0.042870370*sin(-pi+0.430); 
next_target.phi  =  -pi+0.430; 
next_target.vel  =  0.042870370; 

elseif  (next  target.time  >=  10199  &&  next  target.time  <  1 1051) 
next_target.x  =  target.x  +  dt*0.042870370*cos(pi-0.770); 
next_target.y  =  target.y  +  dt*0.042870370*sin(pi-0.770); 
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next_target.phi  =  pi-0.770; 
next_target.vel  =  0.042870370; 

elseif  (next  target.time  >=  11051  &&  nexttarget.time  <  11500) 
nexttarget.x  =  target. x  +  dt*0. 059 160494*cos(pi- 1.360); 
next_target.y  =  target.y  +  dt*0. 059 160494*sin(pi- 1.360); 
nexttarget.phi  =  pi-1.360; 
nexttarget.vel  =  0.059160494; 

elseif  (next_target.time  >=  11500) 
next_target.x  =  target.x; 
next_target.y  =  target.y; 
nexttarget.phi  =  0; 
next_target.vel  =  0; 

end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  generates  trajectories  for  platforms 

function  platforms  =  generate _ platforms(times) 

%  Define  the  global  variables 
globals; 

%  Define  5  fixed  platform  initial  locations 

%  Model  the  MPA  radar  stations 
for  i=l  :NUM_PLATFORMS 
if  i==l 

platforms(  l,i)  =  make_platform; 
platforms!  1  ,i).time=times(  1 ); 
platforms!  l,i).id=i; 
platforms!  l,i).x  =  285.4700000; 
platforms!  l,i).y  =  360.8527778; 
platforms! l,i).phi  =  2*pi*rand  -pi; 
platforms!  l,i).vel  =  0; 
platforms!  l,i).gamma=0; 

%  Now  iterate  for  all  time 
[temp,ntimes]=size(times); 
for  n=2:ntimes 

platforms(n,i)=platform_step(platforms(n- 1  ,i),times(n)); 
end 
end 

if  i==2 

platforms!  l,i)  =  make_platform; 
platforms!  1  ,i)  .time=times(  1 ); 
platforms!  1  ,i).id=i; 
platforms!  l,i).x  =  342.7972222; 
platforms!  l,i).y  =  31 1.9850000; 
platforms! l,i). phi  =  2*pi*rand  -pi; 
platforms!  l,i).vel  =  0; 
platforms!  1  ,i) .  gamma=0; 
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%  Now  iterate  for  all  time 
[temp,ntimes]=size(times); 
for  n=2:ntimes 

platforms(n,i)=platform_step(platforms(n- 1  ,i),times(n)); 
end 
end 

if  i==3 

platforms(l,i)  =  make_platform; 
platforms(l,i)-time=times(l); 
platforms(l,i)-id=i; 
platforms(l,i).x  =  413.1333333; 
platforms(l,i).y  =  349.8194444; 
platforms(l,i)-phi  =  2*pi*rand  -pi; 

platforms(l,i)-vel  =  0; 

platforms(l,i)-gamma=0; 

%  Now  iterate  for  all  time 
[temp  ,ntimes] =size(times) ; 
for  n=2:ntimes 

platforms(n,i)=platfonn_step(platfonns(n- 1  ,i),timesf  n)); 
end 
end 

if  i==4 

platforms(  1  ,i)  =  make_platform; 
platforms(  1  ,i)  .time=times(  1 ); 
platforms(  1  ,i)dd=i; 
platforms(  1  ,i).x  =  46 1 .476 1 1 1 1 ; 
platforms(  1  ,i).y  =  403 .426 1111; 
platforms(l,i)-phi  =  2*pi*rand  -pi; 
platforms(l,i)-vel  =  0; 
platforms(  1  ,i)-gamma=0; 

%  Now  iterate  for  all  time 
[temp,ntimes]=size(times); 
for  n=2:ntimes 

platforms(n,i)=platfonn_step(platforms(n- 1 ,  i  ),ti  rncsfn )); 
end 
end 

if  i==5 

platforms(  1  ,i)  =  make_platform; 
platforms(  1  ,i)  .time=times(  1 ); 
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platforms(l,i)-id=i; 
platforms(l,i)-x  =  517.0844444; 
platforms(l,i).y  =  421.9305556; 
platforms(l,i)-phi  =  2*pi*rand  -pi; 
platforms(l,i)-vel  =  0; 
platforms(  1  ,i)-gamma=0; 

%  Now  iterate  for  all  time 
[temp,ntimes]=size(times); 
for  n=2:ntimes 

platforms(n,i)=platfomi_step(platforms(n- 1 ,  i),ti  mes(  n )); 
end 
end 

%  Define  3  mobile  platform  initial  locations 

%  Model  the  CPC 
if  i==6 

platforms(l,i)  =  make_platform; 
platforms(  1  ,i).time=times(  1 ); 
platforms(  1  ,i)-id=i; 
platforms(l,i).x  =  474.1785056; 
platforms(l,i).y  =  351.7903000; 

platforms(l,i)-phi  =  2*pi*rand  -pi; 
platforms(l,i)-vel  =  0; 

targets(  1 ,  i) .  gamma=0 ; 

%  Now  iterate  for  all  time 
[temp  ,ntimes] =size(times) ; 
for  n=2:ntimes 

platforms(n,i)=platfonn_step_PCG  l_path(platforms(n- 1  ,i),times(n)); 
end 
end 

if  i==7 

platforms(l,i)  =  make_platform; 
platforms(l,i)-time=times(l); 
platforms(  1  ,i).id=i; 
platforms(l,i).x  =  474.1785056; 
platforms(l,i).y  =  351.7903000; 
platforms(l,i)-phi  =  2*pi*rand  -pi; 
platforms(l,i)-vel  =  0; 
targets(  1 ,  i) .  gamma=0 ; 

%  Now  iterate  for  all  time 
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[temp,ntimes]=size(times); 
for  n=2:ntimes 

platforms(n,i)=platfomi_step_PCG2_path(platforais(n-l,i),times(n)); 

end 

end 

%  Model  the  PV 
if  i==8 

platforms(l,i)  =  make_platform; 
platforms(  1  ,i).time=times(  1 ); 
platforms!  1  ,i).id=i; 
platforms!  l,i).x  =  474.1785056; 
platforms!  l,i).y  =  351.7903000; 
platforms! l,i)-phi  =  2*pi*rand  -pi; 
platforms!  l,i)-vel  =  0; 
targets!  1  ,i)  •  gamma=0 ; 

%  Now  iterate  for  all  time 
[temp,ntimes]=size(times); 
for  n=2:ntimes 

platforms!n,i)=platform_stcp_PV_path(platforms(n-l  ,i),timcs!n)); 
end 
end 

%  Given  the  speed  and  sensor  coverage  of  the  MPaA, 

%  its  surveying  of  the  small  TSS  can  be  modeled  as  a  "fixed"  sensor 
%  with  complete  of  the  TSS  during  its  operation. 

%  Model  the  MPaA 
if  i==9 

platforms!  1  ,i)  =  make_platform; 
platforms!  1  ,i)  .time=times(  1 ); 
platforms!  1  ,i)-id=i; 
platforms!  l,i).x  =  474.1785056; 
platforms!  l,i).y  =  351.7903000; 
platforms! l,i).phi  =  2*pi*rand  -pi; 
platforms!  l,i).vel  =  0; 
targets!  1  ,i)-gamma=0; 

%  Now  iterate  for  all  time 
[temp,ntimes]=size(times); 
for  n=2:ntimes 

platforms(n,i)=platfonn_step_MPAC(platforms(n- 1  ,i),times!n)); 
end 
end 
end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  This  function  initialises  a  new  platfonu 

function  new=make_platform 

new=struct('id',0, 'time', 0,'x',0,'yC0, 'phi', 0,'vel',0, 'gamma', 0); 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 
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%  This  is  a  no  motion  platform  model  for  MPA  radar  station 
function  next_platfonn=platform_step(platfonn,time) 
globals; 

dt=time-platform.time; 
next_platfonn  =  make_platfonn; 
next_platfonn.id=platform.id; 
next_platfonn.time  =  time; 
next_platfonn.x  =  platfonn.x; 
next_platfonn.y  =  platform.y; 
next_platfonn.phi  =  platform.phi; 
next_platfonn.vel  =  platform,  vel; 
next_platfonn. gamma  =  platform.gamma; 
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%  This  is  the  platform  motion  model  for  one  of  the  two  CPCs  for  a  day 
function  next_platfonn=platform_step_PCG  l_path(platform,time) 
globals; 

dt=time-platform.time; 
next_platfonn  =  make_platfonn; 
next_platfonn.id=platform.id; 
next_platfonn.time  =  time; 

if  (next_platform.time  >=  0) 
next_platfonn.x  =  474.1785056; 
next_platfonn.y  =  351.7903000; 
next_platfonn.phi  =  platform.phi; 
next_platfonn.vel  =  platform.vel; 
next_platfonn.  gamma  =  platform. gamma; 
end 


% - 

%  This  models  the  platform's  patrol  path.  However,  by  making  the  assumption 
%  that  the  sensors  have  extended  range  to  simplify  sensor  management,  this 
%  modeling  is  not  used  here.  This  will  be  useful  for  future  work  with  the 
%  inclusion  of  sensor  management. 

% 

%  if  (next_platfonn.time  >=  0  &&  next_platfonn.time  <  18000) 

%  next_platform.x  =  platform.x  +  dt*0.015061617*cos(l.  519574188-0. 5*pi); 
%  next_platfonn.y  =  platform.y  +  dt*0.015061617*sin(l.  519574188-0. 5*pi); 
%  next_platfonn.phi  =  1. 519574188-0. 5*pi; 

%  next_platfonn.vel  =  0.015061617; 

%  next_platfonn. gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  18000  &&  next_platform.time  <  32400) 

%  next_platform.x  =  platform.x  +  dt*0.025102695*cos(l.  519574188+0. 5*pi); 
%  next_platfonn.y  =  platform.y  +  dt*0.025102695*sin(l.  519574188+0. 5*pi); 
%  next_platfonn.phi  =  1.5 19574188+0. 5*pi; 

%  next_platfonn.vel  =  0.025 102695; 

%  next_platform.  gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  32400  &&  next_platform.time  <  46800) 

%  next_platfonn.x  =  platform.x  +  dt*0.015061617*cos(l.  5 19574188-0. 5*pi); 
%  next_platform.y  =  platform.y  +  dt*0.0 1506 1 6 1 7*sin(  1 . 519574188-0. 5*pi); 
%  %next_platform.phi  =  1. 519574188-0. 5*pi; 

%  %next_platform.vel  =  0.015061617; 
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%  %next_platform. gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  46800  &&  next_platform.time  <  61200) 

%  next_platform.x  =  platform.x  +  dt*0.025102695*cos(l.  519574188+0. 5*pi); 
%  next_platform.y  =  platform.y  +  dt*0.025102695*sin(l.  519574188+0. 5*pi); 
%  next_platform.phi  =  1. 519574188+0. 5*pi; 

%  next_platform.vel  =  0.025 102695; 

%  next_platform.  gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  61200  &&  next_platform.time  <  75600) 

%  next_platfonn.x  =  platform.x  +  dt*0.015061617*cos(l. 5 19574188-0. 5*pi); 
%  next_platform.y  =  platform.y  +  dt*0.0 1 506 1 6 1 7*sin(  1 . 519574188-0. 5*pi); 
%  %next_platform.phi  =  1. 519574188-0. 5*pi; 

%  %next_platform.  vel  =  0.015061617; 

%  %next_platform. gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  75600  &&  next_platform.time  <  86400) 

%  next_platform.x  =  platform.x  +  dt*0. 025 102695*cos(l.  5 19574188+0. 5*pi); 
%  next_platfonn.y  =  platform.y  +  dt*0.025102695*sin(l. 519574188+0. 5*pi); 
%  next_platform.phi  =  1. 519574188+0. 5*pi; 

%  next_platfonn.vel  =  0.025 102695; 

%  next_platform.  gamma  =  platform,  gamma; 

% 

%  elseif  (next_platform.time  >=  86400) 

%  next_platfonn.x  =  platform.x; 

%  next_platfonn.y  =  platform.y; 

%  next_platform.phi  =  0; 

%  next_platform.vel  =  0; 

%  next_platform.  gamma  =  0; 

%  end 

% - 


290 


%  This  is  the  platform  motion  model  for  one  of  the  two  CPCs  for  a  day 
function  next_platform=platform_step_PCG2_path(platform,time) 
globals; 

dt=time-platform.time; 
next_platfonn  =  make_platfonn; 
next_platfonn.id=platform.id; 
next_platfonn.time  =  time; 

if  (next_platform.time  >=  0) 
next_platfonn.x  =  474.1785056; 
next_platfonn.y  =  351.7903000; 
next_platform.phi  =  platform.phi; 
next_platform.vel  =  platform.vel; 
next_platform.gamma  =  platform. gamma; 
end 


% - 

%  This  models  the  platform's  patrol  path.  However,  by  making  the  assumption 
%  that  the  sensors  have  extended  range  to  simplify  sensor  management,  this 
%  modeling  is  not  used  here.  This  will  be  useful  for  future  work  with  the 
%  inclusion  of  sensor  management. 

% 

%  if  (next_platfonn.time  >=  0  &&  next_platform.time  <  18000) 

%  next_platform.x  =  platform.x  +  dt*0.013140537*cos(-l.  330818664+0. 5*pi); 
%  next_platfonn.y  =  platform.y  +  dt*0.013140537*sin(-l. 330818664+0. 5*pi); 
%  next_platfonn.phi  =  - 1.3308 18664+0. 5  *pi; 

%  next_platfonn.vel  =  0.013 140537; 

%  next_platfonn. gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  18000  &&  next_platform.time  <  32400) 

%  next_platform.x  =  platform.x  +  dt*0. 02 1900894*cos(- 1.3308 18664-0. 5  *pi); 
%  next_platfonn.y  =  platform.y  +  dt*0. 02 1900894*sin(- 1.3308 18664-0. 5  *pi); 
%  next_platfonn.phi  =  -1. 330818664-0. 5*pi; 

%  next_platfonn.vel  =  0.021900894; 

%  next_platform.  gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  32400  &&  next_platform.time  <  46800) 

%  next_platfonn.x  =  platform.x  +  dt*0.013140537*cos(-l. 330818664+0. 5*pi); 
%  next_platform.y  =  platform.y  +  dt*0.013140537*sin(-l. 330818664+0. 5  *pi); 
%  next_platform.phi  =  - 1.3308 18664+0. 5  *pi; 

%  next_platfonn.vel  =  0.013 140537; 
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%  next_platform.  gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  46800  &&  next_platform.time  <  61200) 

%  next_platform.x  =  platform.x  +  dt*0. 02 1900894*cos(- 1.3308 18664-0. 5  *pi); 
%  next_platform.y  =  platform.y  +  dt*0. 02 1900894*sin(- 1.3308 18664-0. 5  *pi); 
%  next_platform.phi  =  -1. 330818664-0. 5*pi; 

%  next_platform.vel  =  0.021900894; 

%  next_platform. gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  61200  &&  next_platform.time  <  75600) 

%  next_platform.x  =  platform.x  +  dt*0.013140537*cos(-l. 330818664+0. 5*pi); 
%  next_platform.y  =  platform.y  +  dt*0.013140537*sin(-l. 330818664+0. 5  *pi); 
%  next_platform.phi  =  - 1.3308 18664+0. 5  *pi; 

%  next_platfonn.vel  =  0.013 140537; 

%  next_platform.  gamma  =  platform,  gamma; 

% 

%  elseif  (next_platform.time  >=  75600  &&  next_platform.time  <  86400) 

%  next_platform.x  =  platform.x  +  dt*0.021900894*cos(-l. 330818664-0. 5*pi); 
%  next_platform.y  =  platform.y  +  dt*0. 02 1900894*sin(- 1.3308 18664-0. 5  *pi); 
%  next_platform.phi  =  -1. 330818664-0. 5*pi; 

%  next_platfonn.vel  =  0.02 1900894; 

%  next_platfonn.  gamma  =  platform,  gamma; 

% 

%  elseif  (next_platform.time  >=  86400) 

%  next_platform.x  =  platform.x; 

%  next_platform.y  =  platform.y; 

%  next_platform.phi  =  0; 

%  next_platform.vel  =  0; 

%  next_platform.  gamma  =  0; 

%  end 

% - 
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%  This  is  the  platform  motion  model  for  PV  for  a  day 
function  next_platform=platform_step_PV_path(platform,time) 
globals; 

dt=time-platform.time; 
next_platfonn  =  make_platfonn; 
next_platfonn.id=platform.id; 
next_platfonn.time  =  time; 

if  (next_platform.time  >=  0) 
next_platfonn.x  =  474.1785056; 
next_platfonn.y  =  351.7903000; 
next_platfonn.phi  =  platform.phi; 
next_platfonn.vel  =  platform.vel; 
next_platfonn.  gamma  =  platform. gamma; 
end 


% - 

%  This  models  the  platform's  patrol  path.  However,  by  making  the  assumption 
%  that  the  sensors  have  extended  range  to  simplify  sensor  management,  this 
%  modeling  is  not  used  here.  This  will  be  useful  for  future  work  with  the 
%  inclusion  of  sensor  management. 

% 

%  if  (next_platfonn.time  >=  0  &&  next_platform.time  <  43200) 

%  next_platform.x  =  platform.x  +  dt*0.013972407*cos(0. 1528 13367); 

%  next_platfonn.y  =  platform.y  +  dt*0.013972407*sin(0. 1528 13367); 

%  next_platfonn.phi  =  0.152813367; 

%  next_platfonn.vel  =  0.013972407; 

%  next_platfonn. gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  43200  &&  next_platform.time  <  86400) 

%  next_platform.x  =  platform.x  +  dt*0.013972407*cos(0.152813367+pi); 

%  next_platform.y  =  platform.y  +  dt*0.013972407*sin(0.152813367+pi); 

%  next_platfonn.phi  =  0.152813367+pi; 

%  next_platfonn.vel  =  0.013972407; 

%  next_platform.  gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  86400) 

%  next_platfonn.x  =  platform.x; 

%  next_platform.y  =  platform.y; 

%  next_platform.phi  =  0; 

%  next_platfonn.vel  =  0; 
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%  next_platform. gamma  =  0; 

%  end 

% - - - 


294 


%  This  is  the  platform  motion  model  for  MPaA 

function  next_platfonn=platform_step_MPAC(platfonn,time) 

globals; 

next_platfonn  =  make_platfonn; 
next_platfonn.id=platform.id; 
next_platfonn.time  =  time; 

if  (next_platform.time  >=  0) 
next_platfonn.x  =  474.1785056; 
next_platfonn.y  =  351.7903000; 
next_platfonn.phi  =  platform.phi; 
next_platfonn.vel  =  platform.vel; 
next_platfonn.  gamma  =  platform. gamma; 
end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  is  to  get  a  fixed  filter  parameter  block 

function  filter=get_filter_params 

globals; 

%  Assume  a  constant  velocity  model; 

filter.XDIM=4; 

filter.  ZD  IM=2; 

filter. Q=  [DTA2 * SIGM A_XDOTA2  0  0  0; 

0  SIGMA_XDOTA2  0  0; 

0  0  DTA2*SIGMA_YDOTA2  0; 

0  0  0  SIGMA_YDOTA2] ; 

filter. H=  [1  0  0  0;  0  0  1  0]; 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  Returns  a  sensor  data  structure  which  contains  all 
%  necessary  infonnation  for  a  decentralised  sensor. 

function  sensor=make _ sensor(id, filter) 

globals; 

%  Define  sensors  for  the  5  fixed  platforms 
if  id==l 
sensor.  id=id; 
sensor.time=0.0; 

sensor.r_err=SIGMA_RANGE_MPA; 
sensor.  b_err=SIGMA_BEARING_MPA; 
sensor.point=0;  %  zero  point  angle 
sensor.beam_view=2*pi; 

sensor.max_range=154;  %  an  equivalent  of  15nm 
sensor.filter=filter; 

sensor.  est=make_track(id,filter.XDIM, 0.0); 
sensor.pred=make_track(id,filter.XDIM,0.0); 
sensor.  num_chans=NUM_CHAN  S ; 
for  i=l  :NUM_CHANS 

sensor.  chan_in(i)=make_track(i, filter  .XDIM, 0.0); 
sensor.  chan_out(i)=make_track(i, filter  .XDIM, 0.0); 
end 


elseif  id==2 
sensor.  id=id; 
sensor.time=0.0; 

sensor.r_err=SIGMA_RANGE_MPA; 
sensor.  b_err=SIGMA_BEARING_MPA; 
sensor.point=0; 
sensor.beam_view=2*pi; 
sensor.max_range=  154; 
sensor.filter=filter; 

sensor.  est=make_track(id,  filter  .XDIM,  0.0); 
sensor.pred=make_track(id,filter.XDIM,0.0); 
sensor.  num_chans=NUM_CHAN  S ; 
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for  i=  1  :NUM_CHAN S 

sensor.  chan_in(i)=make_track(i, filter  .XDIM, 0.0); 
sensor.  chan_out(i)=make_track(i,  filter  .XD IM,  0 . 0 ) ; 
end 


elseif  id==3 
sensor.  id=id; 
sensor.time=0.0; 

sensor.r_err=SIGMA_RANGE_MPA; 
sensor.  b_err=S  IGM  ABE  ARIN  G_MP  A ; 
sensor.point=0; 
sensor.beam_view=2*pi; 
sensor.max_range=  154; 
sensor.  filter=filter; 

sensor.  est=make_track(id,filter.XDIM, 0.0); 
sensor.pred=make_traek(id,filter.XDIM,0.0); 
sensor.  num_chans=NUM_CH  AN  S ; 
for  i=  1  :NUM_CHAN S 

sensor.  chan_in(i)=make_track(i, filter  .XDIM, 0.0); 
sensor.chan_out(i)=make_track(i, filter  .XDIM, 0.0); 
end 


elseif  id==4 
sensor.  id=id; 
sensor.time=0.0; 

sensor.  r_err=S  IGM  A_RAN  GEMP  A ; 
sensor.  b_err=S  IGM  ABE  ARIN  G_MP  A ; 
sensor.point=0; 
sensor.beam_view=2*pi; 
sensor.max_range=  154; 
sensor.  filter=filter; 

sensor.  est=make_track(id, filter  .XDIM, 0.0); 
sensor.pred=make_traek(id,filter.XDIM,0.0); 
sensor.  num_chans=NUM_CH  AN  S ; 
for  i=  1  :NUM_CHAN S 

sensor.  chan_in(i)=make_track(i,  filter  .XDIM,  0.0); 
sensor.  chan_out(i)=make_track(i, filter  .XDIM, 0.0); 
end 


elseif  id==5 
sensor.  id=id; 
sensor.time=0.0; 

sensor.r_err=SIGMA_RANGE_MPA; 
sensor.  b_err=S  IGM  ABE  ARIN  G_MP  A ; 
sensor.point=0; 
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sensor.beam_view=2*pi; 
sensor.max_range=  154; 
sensor.  11  ltcr=  filter; 

sensor.  est=make_track(id, filter  .XDIM, 0.0); 
sensor.pred=make_traek(id,filter.XDIM,0.0); 
sensor.  num_chans=NUM_CH  AN  S ; 
for  i=l  :NUM_CHANS 

sensor.  chan_in(i)=make_track(i, filter  .XDIM, 0.0); 
sensor.  chan_out(i)=make_track(i, filter  .XDIM, 0.0); 
end 

%  Define  sensors  for  the  4  mobile  platforms 

%  Define  the  CPC  sensor 
elseif  id==6 
sensor.  id=id; 
sensor.time=0.0; 

sensor.r_err=SIGMA_RANGE_PCG; 
sensor.  b_err=SIGMA_BEARING_PCG; 
sensor.point=0;  %  zero  point  angle 
sensor.beam_view=2*pi; 

sensor.max_range=1000;  %  range  extended  to  simulate 
%  presence  at  Jurong  Island 
sensor.  filter=filter; 

sensor.  est=make_track(id,  filter  .XDIM,  0.0); 
sensor.pred=make_track(id,filter.XDIM,0.0); 
sensor.  num_chans=NUM_CH  AN  S ; 
for  i=  1  :NUM_CHAN S 

sensor.  chan_in(i)=make_track(i, filter  .XDIM, 0.0); 
sensor.  chan_out(i)=make_track(i, filter  .XDIM, 0.0); 
end 

%  Define  the  CPC  sensor 
elseif  id==7 
sensor.  id=id; 
sensor.time=0.0; 

sensor.r_err=SIGMA_RANGE_PCG; 
sensor.  b_err=SIGMA_BEARING_PCG; 
sensor.point=0; 
sensor.beam_view=2*pi; 
sensor.max_range=  1 000; 
sensor.  filter=filter; 

sensor.  est=make_track(id, filter  .XDIM, 0.0); 
sensor.pred=make_traek(id,filter.XDIM,0.0); 
sensor.  num_chans=NUM_CH  AN  S ; 
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for  i=  1  :NUM_CHAN S 

sensor.  chan_in(i)=make_track(i, filter  .XDIM, 0.0); 
sensor.  chan_out(i)=make_track(i, filter  .XDIM, 0.0); 
end 

%  Define  the  PV  sensor 
elseif  id==8 
sensor.  id=id; 
sensor.time=0.0; 

sensor.r_err=SIGMA_RANGE_PV ; 
sensor.  b_err=SIGMA_BEARING_PV; 
sensor.point=0; 
sensor.beam_view=2*pi; 
sensor.max_range=  1 000; 
sensor.  filter=filter; 

sensor.  est=make_track(id, filter  .XDIM, 0.0); 
sensor.pred=make_traek(id,filter.XDIM,0.0); 
sensor.  num_chans=NUM_CH  AN  S ; 
for  i=  1  :NUM_CHAN S 

sensor.  chan_in(i)=make_track(i,  filter  .XDIM,  0.0); 
sensor.  chan_out(i)=make_track(i, filter  .XDIM, 0.0); 
end 

%  Define  the  MPaA  sensor 
elseif  id==9 
sensor.  id=id; 
sensor.time=0.0; 

sensor.r_err=SIGMA_RANGE_MPAC; 
sensor.  b_err=S  IGM  ABE  ARIN  G_MP  AC ; 
sensor.point=0; 
sensor.beam_view=2*pi; 
sensor.max_range=  1 000; 
sensor.  filter=filter; 

sensor.  est=make_track(id, filter  .XDIM, 0.0); 
sensor.pred=make_traek(id,filter.XDIM,0.0); 
sensor.  num_chans=NUM_CH  AN  S ; 
for  i=  1  :NUM_CHAN S 

sensor.  chan_in(i)=make_track(i, filter  .XDIM, 0.0); 
sensor.  chan_out(i)=make_track(i, filter  .XDIM, 0.0); 
end 
end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  This  function  makes  space  for  a  track 

function  track=make_track(id, dim, time) 

track.id=id; 
track.XD  IM=dim; 
track.time=time; 
track.y=zeros(dim,  1); 
track.Y=zeros(dim,dim); 
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%  This  function  initialises  a  network  structure 


function  network=init_net(platforms, nodes) 

nsize=length(nodes); 

network=ones(nsize,nsize); 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  constructs  a  report  for  a  sensor  at  a  particular  time 

function  report=sensor _ report(sensor, platforms, targets, nsensor,ntime) 

globals; 

report=zeros(NUM_TARGETS,5); 

%  Find  platform  location  at  this  time 

px=platforms(ntime,nsensor).x; 

py=platforms(ntime,nsensor).y; 

for  tnum=l  :NUM_TARGETS 
%  Find  target  location 
tx=targets(ntime,tnum).x; 
ty=targets(ntime,tnum).y; 

%  Compute  range  and  bearing 

dx=tx-px; 

dy=ty-py; 

range=sqrt(dx*dx  +  dy*dy); 

bearing=atan2(dy,dx); 

%  Determine  if  this  is  actually  visable 
if  range  <  sensor.max  range 
%  Add  error  from  sensor  model 
report(tnum,  1  )=tnum; 

report(tnum,2)=range  +  sensor.r_err*(-l+2*rand); 
report(tnum,3)=bearing  +  sensor.b_err*(-l+2*rand); 
report(tnum,4)=px; 
report(tnum,  5  )=py ; 
else 

report(tnum,  1  )=tnum; 
report(tnum,2)=NaN; 
report(tnum,3)=NaN; 
report(tnum,4)=NaN; 
report(tnum,  5  )=N  aN ; 
end 
end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  performs  local  prediction  for  each  node  using  the  CVM 

function  node=local_predict(node,time) 

globals; 

dt=time-node .  time ; 
if  dt  <  0 

error(’Negative  prediction  step  in  state_pred’) 
end 

%  Compute  matrices 
F=[l  dt  0  0; 

0  100; 

0  0  1  dt; 

00  0  1]; 

G=[l  0  0  0; 

0  100; 

00  10; 

00  0  1]; 

Q=node.filter.Q; 

%  Do  prediction 

M=(inv(F))'  *  (node.  est.  Y)  *  (inv(F)); 

Sigma=G'*M*G+inv(Q); 

Omega=M  *  G  *  inv(  S  igma) ; 
node.pred.Y=M-(Omega*Sigma*Omega'); 
node.pred.y=(eye(4,4)-(Omega*G'))*(inv(F))'*(node.est.y); 
node.time=time; 

%  Seems  to  require  for  stability  in  Matlab 
node.pred.Y=force_sym(node.pred.Y); 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  forces  symmetry  on  a  matrix 

function  sigma=force_sym(sigma) 

[nx,ny]=size(sigma); 
if  nx  ~=  ny 

error('Matrix  must  be  square’); 
end 


for  i=l:nx 
for  j=i+l:nx 

temp=(sigma(i,j)+sigma(j,i))/2; 

sigma(i,j)=temp; 

sigma(j,i)=temp; 

end 

end 


305 


%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  perfonns  information  filter  local  update  for  each  node 

function  node=local_update(node,obs,i) 

globals; 

%  Observation  model 
H=node.filter.H; 

if  isnan(obs.report(2:5))  %  Wait  for  observation 
%  Construct  information  terms 
info=zeros(XSIZE,l); 

Info=zero s(XS  IZE ,  XSIZE); 

%  Add  the  information  to  prediction 
node .  est .  y=node .  pred.  y+info ; 
node.est.Y=node.pred.Y+Info; 

%  Seems  to  require  the  next  step  for  stability  in  Matlab 
node. est.Y=force_sym(node. est.  Y); 

elseif  (obs.report(2:5)  ~=  99999)  %  Update  with  observation 
%  Extract  observation 

[zx, zy,R]=xy_obs(obs. report, l,i);  %  R  is  not  used 

z=[zx;zy]; 

if  i==l 

Ri=inv(Rfilter_MPA); 
elseif  i==2 

Ri=inv(Rfilter_MPA); 
elseif  i==3 

Ri=inv(Rlilter_MPA); 
elseif  i==4 

Ri=inv(Rlilter_MPA); 
elseif  i==5 

Ri=inv(Rlilter_MPA); 
elseif  i==6 

Ri=inv(Rfilter_PC  G) ; 
elseif  i==7 

Ri=inv(Rfilter_PCG); 
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elseif  i==8 

Ri=inv(Rfilter_P  V) ; 
elseif  i==9 

Ri=inv(Rfilter_MP  AC) ; 
end 

%  Construct  information  terms 
info=H'*Ri*z; 

Info=H'*Ri*H; 

%  Add  the  information  to  prediction 

node.est.y=node.pred.y+info; 

node.est.Y=node.pred.Y+Info; 

%  Seems  to  require  the  next  step  for  stability  in  Matlab 
node.est.Y=force_sym(node.est.Y); 

elseif  (obs.report(2:5)  ==  99999)  %  Propagate  prediction  if  no  observation 
node.est.y=node.pred.y; 
node.est.Y=node.pred.  Y ; 
end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  constructs  the  sensor  network  connectivity  topology 
function  net=set_net_topology(net, nodes, time, METHOD); 
globals; 

switch  upper(METHOD) 
case  'FIXED'  %  Fixed  net 
return; 

case  'BROADCAST' 

net=ones(NUM_N  ODES  ,NUM_N  ODES) ; 
for  i=  1  :NUM_NODES 
net(i,i)=0;  %  Dynamic  net 
end 

otherwise 

disp('Waming:  Unknown  network  topology  type'); 
net=net  %  Default  is  fixed 
end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  A  function  to  "communicate"  information  between  sensor  nodes 
%  In  general  the  philosophy  is  first  to  identify  the  nodes 
%  in  the  communication  neigbourhood,  then  to  compute  a  common 
%  infomiation  term  locally.  Finally  the  complete  estimate  is 
%  communicated  and  subtraction  of  common  terms  is  done  at  the 
%  receiving  node.  This  approach  makes  the  fusion  process  robust  to 
%  changes  in  communication  topology. 

function  node=communicate(Nodes,com_net,node_num) 

globals; 

%  This  node 

node=Nodes(node_num); 

%  Find  the  current  communication  neighbourhood 
num_chan=l; 
for  i=l  :NUM_NODES 
if  com_net(node_num,i)==T 

%  For  connected  nodes,  compute  common  prior  and  estimate 
%  The  difference  is  the  new  infomiation 
node.chan_out(num_chan)=Nodes(i).pred; 

%  chan  in  contains  the  new  observations 
node.chan_in(num_chan).y=Nodes(i).est.y-Nodes(i).pred.y; 
node.chan_in(num_chan).Y=Nodes(i).est.Y-Nodes(i).pred.Y; 
num_chan=num_chan+ 1 ; 
end 
end 

%  Note  the  number  of  channels  for  later 
nodes. num_chan=num_chan- 1 ; 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  assimilates  all  the  information  from  each  sensor  channel. 

%  The  new  information  is  simply  the  incomming  minus  the  outgoing 
%  infonnation,  or  the  update  minus  the  common  information. 

function  node=assimilate(node,time); 

globals; 

%  Initialisation 
locali=node.est.y-node.pred.y; 
localI=node.est.Y-node.pred.Y; 
yc=node.pred.y; 

Yc=node.pred.Y; 

%  Generate  common  prior  using  covariance  intersection 
if  -FULLCON 

for  i=l  :node.num_chans 

[yc,Yc,omega]=ci_common(node.chan_out(i).y,node.chan_out(i).Y,yc,Yc); 

end 

end 

%  Or  through  fully  connected  assumption 

node.est.y=yc+locali; 

node.est.Y=Y  c+locall; 

for  i=l  :node.num_chans 
node.est.y=node.est.y+node.chan_in(i).y; 
node.est.Y=node.est.Y+node.chan_in(i).Y; 
end 

%  Seems  to  require  the  next  step  for  stability  in  Matlab 
node.est.Y=force_sym(node.est.Y); 

%  This  function  performs  the  covariance  intersect  update  of  two  estimates 
%  (a  with  covariance  A;  b  with  covariance  B)  and  an  observation  matrix  H. 

%  The  output  is  the  output  estimate  (c  with  covariance  C)  and  the  value 
%  of  omega  which  minimizes  the  determinant  of  C. 

function  [yc,Yc, omega]  =  ci_common(ya,Ya,yb,Yb) 
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globals; 


H=[l  0  00; 

00  10; 

1  00  0; 

0  0  10]; 

if  Ya(l)~=0  ||  Yb(l)~=0 

f=inline('l/det(omega*Yai+(l -omega)* Ybi)',’omega',  ’Yai’,  Ybi'); 
Yai=inv(Ya); 

Ybi=inv(Yb); 

YbiH=H'*Ybi; 

YbiHH=YbiH*H; 

omega  =  fminsearch(f,0.5,[],Yai,Ybi); 

Y  ci=Y  ai*omega+YbiHH*(  1  -omega); 

Yc=inv(Yci);  %  New  covariance 

yc=Yc*(Yai*ya+YbiH*yb);  %  New  mean 

else 

Y  c=zeros(XS  IZE,XS  IZE); 
yc=zeros(XSIZE,  1 ); 
omega  =  0; 

end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  displays  true  target  trajectories,  platfonn  trajectories, 

%  target  distinations,  and  sensor  observations  following  a  simulation  run 

function  [true, plat, obs,dest]=post_sim(targets, observations, platforms, destinations) 

globals; 

%  Set  plotting  size 

[nsamps,nplatforms]=size(platforms); 

[nsamps,ndestinations]=size(destinations); 

[nsamp  s  ,ntarget  s] =size(targets) ; 

[nsensors,nsamps]=size(observations); 

figure)  1) 
elf; 

hold  on 

data=zeros(2,nsamps); 
title('True  Target  Motions') 
xlabel('x -position  (m)') 
ylabel(’y-position  (m)') 

%  Plot  true  target  trajectories 
for  n=l:ntargets 
for  i=l:nsamps 
data(l  ,i)=targets(i,n).x; 
data(2,i)=targets(i,n).y; 
end 

true=plot(data(  1 , : ), data(2 , : ) ,  'b -') ; 
end 

%  Plot  true  platform  trajectories 
for  n=T  mplatforms 
for  f=T :  nsamp  s 
data(l  ,i)=platforms(i,n).x; 
data(2,i)=platforms(i,n).y; 
end 

if  n  ==  1 
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plat=plot(data(l,:),data(2,:),'ko’); 
elseif  n  ==  2 

plat=plot(data(l,:),data(2,:),'ko’); 
elseif  n  ==  3 

plat=plot(data(l,:),data(2,:),’ko’); 
elseif  n  ==  4 

plat=plot(data(l,:),data(2,:),'ko'); 
elseif  n  ==  5 

plat=plot(data(l,:),data(2,:),'ko’); 
elseif  n  ==  6 

plat=plot(data(l,:),data(2,:),'r*'); 
elseif  n  ==  7 

plat=plot(data(l,:),data(2,:),’r*'); 
elseif  n  ==  8 

plat=plot(data(l,:),data(2,:),'gh'); 
elseif  n  ==  9 

plat=plot(data(l,:),data(2,:),’bo’); 

end 

end 

%  Plot  true  destination  locations 
for  n=l  rndestinations 
for  i=  I  :nsamps 
data(  1  ,i)=destinations(i,n).x; 
data(2,i)=destinations(i,n).y; 
end 

dest=plot(data(  1  ,:),data(2,:),'rh'); 
end 

%  Plot  sensor  observations 
for  n=l:nsensors  %  for  all  sensors 
for  i=l  :nsamps  %  for  all  time 
report=observations(n,i).report; 
for  m=l  mtargets  %  for  all  targets 
if  report(m,l)  >  0  %  if  they  are  seen 

[zx,zy,R]=xy_obs(report,m,n);  %  convert  observation  to  global  xy  coordinates 
odata(l,m)=zx; 
odata(2,m)=zy; 
end 
end 

if  n  ==  1 

obs=plot(odata(l,:),odata(2,:),’k.'); 
elseif  n  ==  2 

obs=plot(odata(l,:),odata(2,:),'k.'); 
elseif  n  ==  3 
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obs=plot(odata(l,:),odata(2,:),'k.'); 
elseif  n  ==  4 

obs=plot(odata(l,:),odata(2,:),’k.'); 
elseif  n  ==  5 

obs=plot(odata(l,:),odata(2,:),'k.'); 
elseif  n  ==  6 

obs=plot(odata(l,:),odata(2,:),’r.'); 
elseif  n  ==  7 

obs=plot(odata(l,:),odata(2,:),'r.'); 
elseif  n  ==  8 

obs=plot(odata(l,:),odata(2,:),’g.'); 
elseif  n  ==  9 

obs=plot(odata(l,:),odata(2,:),'b.'); 

end 

end 

end 

legend([true, plat, obs,dest], ’Target  True  Position’,’Tracking 

Observations','Intended  Target  Destination') 
hold  off 


Stations','Target 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  extract  an  xy  observation  from  a  range-bearing  report 

function  [zx,zy,R]=xy_obs(rep,n,sensor_id) 

globals; 

sr=sin(rep(n,3)); 
cr=cos(rep(n,3)); 
zx=rep(n,4)+rep(n,2)  *  cr; 
zy=rep(n,  5  )+rep(n,2)  *  sr ; 

ROT=[cr  -sr;  sr  cr]; 
rangc2=rep(n,2)*rep(n,2); 
if  sensor_id==l 

sigma=  [ S IGM A_RAN GE_MP AA2  0;0  range2*SIGMA_BEARING_MPAA2]; 
elseif  sensor_id==2 

sigma=  [ S IGM A_RAN GE_MP AA2  0;0  range2 * SIGMA_BEARIN G_MP AA2] ; 
elseif  sensor_id==3 

sigma=  [ S IGM A_RAN GE_MP A A2  0;0  range2*SIGMA_BEARING_MPAA2]; 
elseif  sensor_id==4 

sigma=  [ S IGM A_RAN GE_MP AA2  0;0  range2 * SIGMA_BEARIN G_MP AA2] ; 
elseif  sensor_id==5 

sigma=  [ S IGM A_RAN GE_MP A A2  0;0  range2*SIGMA_BEARING_MPAA2]; 
elseif  sensor_id==6 

sigma=[SIGMA_RANGE_PCGA2  0;0  range2 * SIGMA_BEARIN G_PCGA2] ; 
elseif  sensor_id==7 

sigma=[SIGMA_RANGE_PCGA2  0;0  range2 *  SIGMA_BEARIN G_PCGA2] ; 
elseif  sensor_id==8 

sigma=  [ S IGM A_RAN GE_P VA2  0;0  range2*SIGMA_BEARING_PVA2]; 
elseif  sensor_id==9 

sigma=  [ S IGM A_RAN GE_MP AC A2  0;0  range2 * S IGM A_BE ARIN G_MP AC A2 ] ; 
end 

R=ROT*sigma*ROT’; 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  displays  the  fused  target  track  following  a  simulation  run 

function  plot_tracks(targets,y,Y, times) 

globals; 

%  Set  plotting  size 

[ntimes,nsensors,xdim  1  ]=size(y); 

[ntimes,nsensors,xdim2,xdim3]=size(Y); 

odata=zeros(2,ntimes); 

title('Target  Motions') 

xlabel(’x-position  (m)') 

ylabel(’y-position  (m)') 

xlim([150  750]) 

ylim([280  440]) 

hold  on 

warning  off; 

s=l; 

for  i=l:ntimes 
P=inv(squeeze(Y(i,s, :,:))); 
x=P*squeeze(y(i,s,:)); 
odata(l,i,s)=x(l); 
odata(2,i,s)=x(3); 
end 

plot(odata(  1 , : ,  1  ),odata(2 , : ,  1  ),'g-') 
hold  off 
warning  on; 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  produces  various  plots  with  regards  to  the  SAW  parameters 
function  plot_errors(targets,y,Y,yp,Yp, times, node_num,nfigure, trackstarttime) 
globals; 

%  Initialise 

[ntimes,nsensors,xdim  1  ]=size(y); 

[ntimes,nsensors,xdim2,xdim3]=size(Y); 
data=zeros(  1 0,ntimes); 
pdata=zeros(10,ntimes);  %predicted  data 
edata=zeros(10,ntimes);  %estimated  data 
tdata=zeros(10,ntimes);  %true  data 
esttimesigma=zeros(  1  ,ntimes); 
ntarget  =  NUMTARGETS; 
xmin  =  4500; 
xmax  =  MAXTIME; 

warning  off; 

%  Calculating  and  storing  the  data  for  each  time-step 
for  i=l:ntimes 

P=inv(squeeze(Y  (i,node_num, :,:))); 
x=P*squeeze(y(i,node_num,:)); 

Pp=inv(squeeze(Yp(i,node_num, :,:))); 
xp=Pp*squeeze(yp(i,node_num,:)); 

data(  1  ,i)=times(i); 

data(2,i)=(x(l)-targets(i,  ntarget).  x)*  1 80; 
data(3,i)=(x(3)-targets(i,  ntarget).  y)*  1 80; 
sigma=real(sqrt(P)); 
data(4,i)=sigma(l,l)*180; 
data(5,i)=sigma(3,3)*  1 80; 

data(6,i)=(real(sqrt(x(2)*x(2)+x(4)*x(4)))-targets(i, ntarget).  vel)*  1 80; 
data(7,i)=atan2(x(4),x(2))-targets(i,ntarget).phi; 
if  abs(data(7,i))  >  pi 
data(7,i)  =  2*pi-abs(data(7,i)); 
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end 

data(8,i)=sqrt(sigma(l,l)*sigma(l,l)+sigma(3,3)*sigma(3,3))*180; 
data(9,i)=sqrt(sigma(2,2)*sigma(2,2)+sigma(4,4)*sigma(4,4))*180; 
data(10,i)=real(sqrt(((x(2)/(x(2)A2  +  x(4)A2))A2)*(sigma(4,4)A2)... 
+((x(4)/(x(2)A2  +  x(4)A2))A2)*(sigma(2,2)A2))); 

tdata(  1  ,i)=times(i); 
tdata(2,i)=targets(i,ntarget).x*  1 80; 
tdata(3,i)=targets(i,ntarget).y*  1 80; 
tdata(6,i)=targets(i,ntarget).vel*  1 80; 
tdata(7,i)=targets(i,ntarget).phi; 

pdata(  1  ,i)=times(i); 
pdata(2,i)=xp(  1)*  1 80; 
pdata(3,i)=xp(3)*180; 
psigma=real(sqrt(Pp)); 
pdata(4,i)=psigma(  1 , 1)*  1 80; 
pdata(5,i)=psigma(3,3)*180; 

pdata(6,i)=(real(sqrt(xp(2)*xp(2)+xp(4)*xp(4))))*  1 80; 
pdata(7,i)=atan2(xp(4),xp(2)); 
if  abs(pdata(7,i))  >  pi 
pdata(7,i)  =  2*pi-abs(pdata(7,i)); 
end 

pdata(8,i)=sqrt(psigma(l,l)*psigma(l,l)+psigma(3,3)*psigma(3,3))*180; 
pdata(9,i)=sqrt(psigma(2,2)*psigma(2,2)+psigma(4,4)*psigma(4,4))*180; 
pdata(10,i)=real(sqrt(((xp(2)/(xp(2)A2  +  xp(4)A2))A2)*(psigma(4,4)A2)... 
+((xp(4)/(xp(2)A2  +  xp(4)A2))A2)*(psigma(2,2)A2))); 

edata(  1  ,i)=times(i); 
edata(2,i)=x(l)*180; 
edata(3,i)=x(3)*180; 
esigma=real(sqrt(P)); 
edata(4,i)=esigma(  1 , 1)*  1 80; 
edata(5,i)=esigma(3,3)*180; 
edata(6,i)=(real(sqrt(x(2)*x(2)+x(4)*x(4))))* 1 80; 
edata(7,i)=atan2(x(4),x(2)); 
if  abs(edata(7,i))  >  pi 
edata(7,i)  =  2*pi-abs(edata(7,i)); 
end 

edata(8  ,i)=sqrt(esigma(  1,1)*  esigma(  1 , 1  )+esigma(3 ,3)  *  esigma(3 ,3  ))  *  1 80 ; 
edata(9,i)=sqrt(esigma(2,2)*esigma(2,2)+esigma(4,4)*esigma(4,4))*180; 
edata(10,i)=real(sqrt(((x(2)/(x(2)A2  +  x(4)A2))A2)*(esigma(4,4)A2)... 
+((x(4)/(x(2)A2  +  x(4)A2))A2)*(esigma(2,2)A2))); 

esttimesigma(  1  ,i)=sqrt(edata(8,i)A2+. . . 


318 


(kdata(6,i)A2)*(edata(9,i)A2))/... 

edata(6,i); 

end 

%  merging  the  heading  standard  deviations  for  plotting 
p=l;q=l;i=l; 
for  j = 1 :  (ntimes)  *  2 
if  j/2-round(j/2)==0 
combtimedata(l,j)=  times(i); 
combdata(  8  ,j  )=edata(  8  ,p) ; 
combdata(9,j)=edata(9,p); 
combdata(  1 0,j)=edata(  1 0,p); 
p=p+l; 

i=i+l; 

else 

combtimedata(l,j)=  times(i); 
combdata(8,j)=pdata(8,q); 
combdata(9,j)=pdata(9,q); 
combdata(  1 0,j)=pdata(  1 0,q); 
q=q+l; 
end 
end 

%  Plot 

figure(nfigure); 

elf; 

plot(data(l,:),abs(data(2,:)),'b’,data(l,:),data(4,:),'r') 
legend('Estimated  Error', 'Standard  Deviation'); 
buf=sprintf('Error  between  X-component  Estimate  and  Truth'); 
title(buf) 

xlim([xmin  xmax]) 
axis  'auto  y' 

xlabel('Simulated  Time  (s)') 
ylabel('X  error  (m)') 

figure(nfigure+ 1 ); 
elf; 

plot(data(T,:),abs(data(3,:)),’b’,data(l,:),data(5,:),'r') 
legend('Estimated  Error', 'Standard  Deviation’); 
buf=sprintf('Error  between  Y-component  Estimate  and  Truth'); 
title(buf) 

xlim([xmin  xmax]) 
axis  'auto  y' 

xlabel('Simulated  Time  (s)') 
ylabel('Y  error  (m)') 
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figure(nfigure+2 ) ; 
elf; 

plot(data(l,:),abs(data(6,:)),’b’,data(l,:),data(9,:),'r') 
legend('Estimated  Error', 'Standard  Deviation’); 
buf=sprintf(’Error  between  Velocity  Estimate  and  Truth’); 
title(buf) 

xlim([xmin  xmax]) 
axis  ’auto  y’ 

xlabel(’Simulated  Time  (s)’) 
ylabel(’Velocity  error  (m/s)’) 

figure(nfigure+3 ) ; 
elf; 

plot(data(T,:),abs(data(7,:)),’b’,data(l,:),data(10,:),'r’) 
legend(’Estimated  Error’, 'Standard  Deviation’); 
buf=sprintf(’Error  between  Heading  Estimate  and  Truth’); 
title(buf) 

xlim([xmin  xmax]) 
axis  ’auto  y’ 

xlabel(’Simulated  Time  (s)’) 
ylabel(’Heading  error  (rads)’) 

figure(nfigure+4) ; 
elf; 

plot(pdata(l,:),pdata(2,:),'r’,edata(l,:),edata(2,:),’g’,  tdata(l,:),tdata(2,:),’b’) 
legend(’Predicted  X’, ’Estimated  X’,’True  X’); 
buf=sprintf(’X  position’); 
title(buf) 

xlim([xmin  xmax]) 
axis  ’auto  y’ 

xlabel(’Simulated  Time  (s)’) 
ylabel(’X  Position  (m)’) 

figure(nfigure+5  ) ; 
elf; 

plot(pdata(l,:),pdata(3,:),'r’,edata(l,:),edata(3,:),’g’,  tdata(l,:),tdata(3,:),’b’) 
legend(’Predicted  Y’, ’Estimated  Y’,’True  Y’); 
buf=sprintf(’Y  position’); 
title(buf) 

xlim([xmin  xmax]) 
axis  'auto  y’ 

xlabel(’Simulated  Time  (s)’) 
ylabel(’Y  Position  (m)’) 
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figure(nfigure+6) ; 
elf; 

plot(pdata(l,:),pdata(6,:),'r',edata(l,:),edata(6,:),'g',  tdata(l,:),tdata(6,:),'b') 
legend('Predicted  VelocityVEstimated  Velocity','True  Velocity'); 
buf=sprintf('V  elocity'); 
title(buf) 

xlim([xmin  xmax]) 
axis  'auto  y' 

xlabel(’Simulated  Time  (s)') 
ylabel('Velocity  (m/s)’) 

figure(nfigure+7) ; 
elf; 

plot(pdata(l,:),pdata(7,:),’r',edata(l,:),edata(7,:),'g',  tdata(l,:),tdata(7,:),'b') 
legend('Predicted  Heading'/Estimated  Heading','True  Heading'); 
buf=sprintf('Heading'); 
title(buf) 

xlim([xmin  xmax]) 
axis  'auto  y' 

xlabel('Simulated  Time  (s)') 
ylabel('Heading  (rad)') 

figure(nfigure+8); 

elf; 

plot(pdata(l,:),pdata(8,:),'r+',edata(l,:),edata(8,:),'g*',combtimedata(l,:),combdata(8,:),’b’) 

legend('Predicted','Estimated'); 

buf=sprintf('Radius  Standard  Deviations'); 

title(buf) 

xlim([xmin  xmax]) 
axis  'auto  y' 

xlabel(’Simulated  Time  (s)') 
ylabel(’Radius  (m)') 

figure(nfigure+9) ; 
elf; 

plot(pdata(l,:),pdata(9,:),'r+',edata(l,:),edata(9,:),’g*',combtimedata(l,:),combdata(9,:),'b') 

legend('Predicted','Estimated'); 

buf=sprintf('Velocity  Standard  Deviations'); 

title(buf) 

xlim([xmin  xmax]) 
axis  'auto  y' 

xlabel('Simulated  Time  (s)') 
ylabel('Velocity  (m/s)') 

figure(nfigure+ 1 0); 
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elf; 

plot(pdata(  1  ,:),pdata(  10,:  ),'r+',edata(  1 ,  :),edata(  1 0,:),'g*  ’,combtimedata(  1 ,  :),combdata(  1 0, 

)/b’) 

legend(’Predicted',’Estimated'); 
buf=sprintf('Heading  Standard  Deviations'); 
title(buf) 

xlim([xmin  xmax]) 
axis  'auto  y' 

xlabel(’Simulated  Time  (s)') 
ylabel(’Heading  (rad)’) 

figure(nfigure+l  1); 
elf; 

plot(edata(  1  ,:),abs((MAX_TIME-edata(l  ,:))- 
kdata(6 , : , 'b edata(  1 , : ), esttimesigma(  1 , : 'r'  ) 
legend('Estimated  Error', 'Standard  Deviation’); 
buf=sprintf('Error  between  Time  Estimate  and  Truth’); 
title(buf) 

xlim([min(kdata(7, 1 000 :MAX_TIME/ 1 0))*DT  xmax- 1  *DT]) 

axis  'auto  y' 

xlabel('Simulated  Time  (s)') 
ylabel('Time  (s)') 

warning  on; 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  This  function  plots  the  network  nodes  and  communication  links 

function  plot_coms(platforms,com_net) 

globals; 

[nsamps,nplatforms]=size(platforms); 

figure(14) 

elf; 

hold  on 

data=zeros(2,nplatforms); 

ploc=zeros(2,2); 

title('Platforms  and  Communication  Links') 
xlabel('x-position  (m)') 
ylabel('y-position  (m)') 

for  n=l  mplatforms 
data(  1  ,n)=platforms(  1  ,n).x; 
data(2,n)=platforms(  1  ,n).y; 
end 

plat=plot(data(  1  ,:),data(2,:),'ko’); 

for  i=l  mplatforms 
for  j=i:nplatforms 
if  com_net(i,j) 
ploc(l,l)=data(l,i); 
ploc(  1 ,2)=data(  1  ,j); 
ploc(2, 1  )=data(2,i); 
ploc(2,2)=data(2,j); 
end 

links=plot(ploc(l,:),ploc(2,:),'r'); 

end 

end 

legend([plat, links], 'SensorsVCommunication  Links'); 
hold  off 
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D.  BLOCK  4.  HYBRID  DATA  &  TRACK  FUSION  ARCHITECTURE  USING 
INFORMATION  FILTER  &  TRACK-TO-TRACK  ALGORITHM 

%  This  file  executes  the  Monte-Carlo  simulation 

%  Number  of  Monte-Carlo  simulation  runs 
totalruns  =  50; 

%  Initialise  variables 
sumfirstleftTSS(l  Totalruns)  =  NaN; 
sumfirstleftTSSntoJIHigh(l  Totalruns)  =  NaN; 
sumfirstleftTSSntoJIModerate(  1  Totalruns)  =  NaN; 
sumfirstleftPL(  1  Totalruns)  =  NaN; 
sumfirstleftPLnto  JIHigh(  1  Totalruns)  =  NaN; 
sumfirstleftPLntoJIModerate(l  Totalruns)  =  NaN; 

%  Execute  each  run  by  generating  new  observations  or  using  previous  ones 
%  Produce  the  fused  track  for  each  run 
%  Then  pool  the  tracks  together  for  analysis 
for  run  =1  Totalruns 

buf=sprintf('Run:  =%d',run);  disp(buf); 
example_sim;  %generate  new  observations 

datastore9(run).observations=observations;  %  store  observations  to  be  executed  by 
other  filters 

observations  =  datastore9(run). observations; 

NEW_RUN  =  0; 
runindep; 

sumfirstleftTSS(run)  =  firstleftTSS; 
sumfirstleftTSSntoJIHigh(run)  =  firstleftTSSntoJIHigh; 
sumfirstleftTSSntoJIModerate(run)  =  firstleftTSSntoJIModerate; 
sumfirstleftPL(run)  =  firstleftPL; 
sumfirstleftPLntoJIHigh(run)  =  firstleftPLntoJIHigh; 
sumfirstleftPLntoJIModerate(run)  =  firstleftPLntoJIModerate; 
sumkdata(run,:,:)  =  kdata; 
sumesttimesigma(run,l,:)  =  esttimesigma; 
lowest(run)  =  min(sumkdata(run,7,1000:MAX_TIME/10)); 
end 

%  Calculate  Probability  of  Impact 
Cxtemp  =  squeeze(sumkdata(:,8,:)); 

Cytemp  =  squeeze(sumkdata(:,9,:)); 

Xtemp  =  squeeze(sumkdata(:,10,:)); 

Ytemp  =  squeeze(sumkdata(:,l  1,:)); 
probability  =  zeros(l,nt); 
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warning  off; 
for  1  =  1  :nt 

Cx  =  diag(Cxtemp(:,l))*(180A2); 

Cy=  diag(Cxtemp(:,l))*(180A2); 

X  =  (Xtemp(:,l)-  302.1 144444)*  180; 

Y  =  (Ytemp(:,l)-  347.7716667)*  180; 

W  =  ones(totalruns,l); 
sigma_xbar_tgo  =  inv((W')*inv(Cx)*W); 
sigma_ybar_tgo  =  inv((W')*inv(Cy)*W); 
sigma  =  sqrt(sigma_xbar_tgo); 

xbar_tgo  =  sigma_xbar_tgo*(((W')*inv(Cx)*X));%  Calculate  mean  impact  pt 
ybar_tgo  =  sigma_ybar_tgo*(((W')*inv(Cy)*Y));%  Calculate  mean  impact  pt 
r_not  =  sqrt(xbar_tgoA2  +  ybar_tgoA2); 

R  =  300; 

g  =  zeros(l,100); 
h  =  zeros(l,100); 
probtemp  =  0; 

recur  =  1 ; 

g_not  =  exp(-(r_notA2)/(2*sigmaA2));  %  g_not 
h_not  =  1  -  exp(-(RA2)/(2*sigmaA2));  %  h  not 
probtempnot  =  g_not*h_not; 

g(l)  =  (l/l)*((r_notA2)/(2*sigmaA2))*g_not; 

h(l)  =  -(l/factorial(l))*(((RA2)/(2*sigmaA2))A(l))*exp(-(RA2)/(2*sigmaA2))  +  h_not; 
probtemp  =  probtempnot  +  g(l)*h(l); 

while  (recur  <=  100) 

g(recur+l)  =  (l/(recur+l))*((r_notA2)/(2*sigmaA2))*g(recur); 
h(recur+ 1 )  =  (- 1  /  factorial(recur+ 1  ))*  (((RA2)/ (2  *  sigmaA2))A(recur+ 1 ))  *  exp(- 

(RA2)/(2*sigmaA2))  +  h(recur); 

probtemp  =  probtemp  +  g(recur+l)*h(recur+l); 
recur  =  recur  +  1 ; 
end 

probability(l,l)  =  probtemp; 
end 

%  Initialise  variables 
count  1  =  0; 
count2  =  0; 
count3  =  0; 
count4  =  0; 
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count5  =  0; 
count6  =  0; 

SsumfirstleftTSS  =  0; 

Ssumfi  rstlcftTSSntoJIHigh  =  0; 

S  sum  (1  rstl  c  f  IT  S  S  n  toJ  I M  odcratc  =  0; 

SsumfirstleftPL  =  0; 

Ssumfl  rstlcftPLntoJIHigh  =  0; 

Ssumfl  rstlcftPLnto.il  Moderate  =  0; 

%  Take  the  average  of  the  pooled  tracks 
for  m  =  1  :totalruns 

if  ~isnan(sumfirstleftTSS(m)) 
count  1  =  count  1+1; 

Ssumfl  rstlcftTSS=SsumfirstleftTSS+sum  firstlcftTSS(m); 
end 

if  ~isnan(sumflrstlcf'tTSSntoJIHigh(m)) 
count2  =  count2+l; 

SsumfirstlcftTSSntoJIHigh=SsumfirstlcftTSSntoJIHigh+sumfirstlcftTSSntoJIHigh(m); 

end 

if  ~isnan(sumfirstlcftTSSntoJ  I  Moderate!  m)) 
count3  =  count3+l; 

Ssumfl  rstlcftTSSntoJIModeratc=Ssum  fl  rstlcf'tTSSntoJIModeratc+sum  fl  rstlcftTSSntoJIM 
oderate(m); 
end 

if  ~i  snan(  sum  fl  rstl  c  f'tP  L( m )  ) 
count4  =  count4+l; 

S  sumfirstleftPL=S  sumfirstleftPL+sumfirstleftPL(m) ; 
end 

if  ~isnan(sumfirstlcftPLntoJIHigh(m)) 
count5  =  count5+l; 

Ssumfl  rstlcftPLntoJlHigh=SsumfirstlcftPLntoJlHigh+sum  firstleftPLntoJlHigh(m); 
end 

if  ~i  snanfsum  fl  rstl  c  f  IP  Ln  to  J I M  oderatefm )) 
count6  =  count6+l; 

SsumfirstlcftPLntoJIModeratc^SsumfirstlcftPLntoJIModcratc+sumfirstlcftPLntoJIModer 

ate(m); 

end 
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end 


mean  fir stlc RTSS  =  SsumllrstlcftTSS/count  1 ; 

meanfirstleftTSSntoJIHigh  =  SsumfirstleftTSSntoJTHigh/count2; 

m  can  (1  rstl  c  ftTS  S  n  to  J I M  o  derate  =  SsumfirstleftTSSntoJIModerate/count3; 

mcanfirstlcftPL  =  SsumfirstleftPL/count4; 

m  can  11  rstl  c  ft  P  Ln  to  J 1 H  i  gh  =  SsumfirstleftPLntoJIHigh/count5; 

meanfirstleftPLntoJIModerate  =  S  s  urn  (1  rstl  c  ft  P  Ln  to  J I M  odcratc/  co  un  t6 ; 

meantruetimedata  =  mean(sumkdata(:,l,:)); 

meandistuncert  =  mean(sumkdata(:,2,:)); 

meandistapart  =  mean(sumkdata(:,3,:)); 

meanesttimedata  =  mean(sumkdata(:,6,:)); 

%  Calculate  the  sample  variance  of  the  estimated  time  to  impact 
for  p  =  1  :nt 
sumtimediff=0; 
timediff=0; 
for  q  =  1  :totalruns 

timediff  =  (sumkdata(q,6,p)-meanesttimedata(p))A2; 
sumtimediff=sumtimediff+timediff; 
end 

meansampletimesigma(p)  =  sqrt(sumtimediff  /(totalruns-1)); 
end 

figure(30); 

elf; 

plot(meantruetimedata(:)',meandistuncert(:)Vb',meantruetimedata(:)',meandistapart(:)Vg') 
legend('Distance  Uncertainty',' Distance  Apart'); 
buf=sprintf(’Estimated  End  Point'); 
title(buf) 

xlim([min(lowest)*DT  MAX  TIME- 1  *DT]) 
ylim(  [0  1000]) 
xlabel('Simulated  Time  (s)') 
ylabel('Distance  (m)') 

figure(31); 

elf; 

plot(meantruetimedata(:)',abs((MAX_TIME-meantruetimedata(:)')- 
meanesttimedata(:)'),'b’,meantruetimedata(:)',meansampletimesigma,'r' ) 
legend('Estimated  Error', 'Standard  Deviation'); 
buf=sprintf('Error  between  Time  Estimate  and  Truth'); 
title(buf) 

xlim([min(lowest)*DT  MAX  TIME- 1  *DT]) 
ylim(  [0  100]) 

xlabel('Simulated  Time  (s)') 
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ylabel('Time  (s)') 

figure(32); 

elf; 

plot(meantruetimedata(:)’,probability(:),'k') 
legend('Probability  of  Impact'); 
buf=sprintf(’Probability  of  Impact’); 
title(buf) 

xlim([min(lowest)*DT  MAX  TIME- 1  *DT]) 
ylim([0  1]) 

xlabel('Simulated  Time  (s)') 
ylabel('Probability  of  Impact') 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  file  runs  the  track-to-track  algorithm.  The  default  condition  is  to 
%  do  a  new  run  every  time.  There  is  also  the  option  to  use  previous 
%  observation  runs  so  that  different  filters  can  be  compared. 

globals; 

ginit; 

if  ~exist('NEW_RUN','var') 

NEW_RUN=1; 

end 

%  If  we  choose  to  use  previous  data, 

%  then  the  existence  of  various  variables  needs  to  be  checked 
if  ~NEW_RUN 

if  ~exist(’observations','var')  |  ~exist(’targets’,'var')  |  ... 

~exist('platforms','var’)  |  ~exist(’times','var') 
error('No  existing  Data  to  perform  simulation'); 
end 

[num_nodes,num_times]=size(observations); 

nt=length(times); 

if  (num_nodes  ~=  NUM_NODES)  |  (numtimcs  ~=  nt) 
error(’Observation  Record  is  of  incorrect  dimension’); 
end 

if  nt~=  length(targets) 
error('Target  set  is  of  incorrect  dimension'); 
end 

[num_times,num_plat]=size(platforms); 
if  (num_nodes  ~=  num_plat)  |  (num  timcs  ~=  nt) 
error('Platform  set  is  of  incorrect  dimension'); 
end 

buf=sprintf('01d  Run  Commencing  with  %d  nodes  and  %d 
steps\n',num_nodes,num_times); 

disp(buf); 

end 

%  If  we  are  doing  a  new  run  then  simulate  the  target  and  platfonn  motions 
ifNEW  RUN 


time 
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clear  all; 

globals; 

ginit; 

NEW_RUN=1; 

times=0:DT:MAX_TIME; 

num_times=length(times); 

buf=sprintf('New  Run  Commencing  with  %d  nodes  and  %d 
step  s\n’,NUM_N  ODE  S  ,num_times- 1 ); 
disp(buf); 

%  This  code  currently  apply  to  only  one  target 
%  One  target  considerably  simplifies  computation 
buf=sprintf('Generating  Target  Set\n');  disp(buf); 
targets=generate _ targets(times); 

%  Simulate  platforms  motion,  assuming  they  are  known 
buf=sprintf(’Generating  Platform  Trajectories\n');  disp(buf); 
platforms=generate _ platforms(times); 

%  Generate  all  target's  attack  destinations 

destinations=generate _ destinations(times); 

else 

%  If  we  are  using  previous  data  set 

%  then  there  is  no  need  to  simulate  targets  or  platforms 

ginit; 

clear  Nodes; 
end 

%  In  every  case,  the  sensors  which  sit  on  the  platforms  are  defined 
%  Also,  this  allows  the  filters  to  be  changed  between  runs 
buf=sprintf('Defining  SensorsVf);  disp(buf); 
filter=get_filter_params; 

for  i=l  :NUM_NODES 

Nodes(i)=make _ sensor(i, filter); 

end 

%  Establish  initial  topology 
com_net=eye(NUM_NODES); 

%  Establish  the  time  base 
ntimes=length(times); 

%  Define  space  to  record  results 
Results_y=zeros(ntimes,  NUM_NODES,  filter  .XDIM); 


time 
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Resuks_Y=zeros(ntimes,NUM_NODES, filter  .XDIM, filter  .XDIM); 


% - 

% 

%  START  OF  FILTER  LOOPS  HERE 

% 

% - 


%  Run  loop  for  all  sensors  at  each  time. 

%  This  is  as  close  as  we  can  get  to  running  this  in  parallel 

for  n=l:ntimes 
kdata(l,n)=0; 
kdata(2,n)=0; 
kdata(3,n)=0; 
kdata(4,n)=0; 
kdata(5,n)=0; 
kdata(6,n)=0; 
kdata(7,n)=99999; 
kdata(8,n)  =  0; 
kdata(9,n)  =  0; 
kdata(10,n)  =  0; 
kdata(l  l,n)  =  0; 
leftT  S  S  (n)=N  aN ; 
leftTSSntoJIHigh(n)=NaN; 
leftTSSntoJIModerate(n)=NaN; 
leftPL(n)=NaN; 
leftPLnto  JIHigh(n)=NaN ; 
leftPLnto  JIModerate(n)=NaN ; 
end 

distapart  =  0; 
distuncert  =  0; 
disttoreach  =  0; 
speedtoreach  =  0; 
timetoreach  =  0; 

for  i=T:ntimes 
startplot=99999; 

buf=sprintf('Step:  time=%d’,i-l);  disp(buf); 

%  Time  in  this  loop 
t=times(i); 

%  Generate  an  observation  for  each  node  at  this  time 
for  s=l:NUM  NODES 
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if  NEW_RUN  %  new  observation  simulation  required 

obs(s).report=sensor _ report(Nodes(s),  platforms,  targets,  s,i); 

observations^, i)=obs(s);  %  record  observations  for  plotting 
else  %  use  old  observation  data 
obs(s)=observations(s,i); 
end 
end 

%  First,  do  a  local  prediction 
for  s=  1  :NUM_NODES 
Nodes(s)=local_predict(Nodes(s),t); 
end 

%  Second,  a  local  update  generating  y  tilde  at  each  site 
for  s=  1  :NUM_NODES 
Nodes(s)=local_update(Nodes(s),obs(s),s); 
end 

%  Record  results 
for  s=  1  :NUM_NODES 
Results_y(i,s,:)=Nodes(s).est.y; 

Results_Y(i,s,:,:)=Nodes(s).est.Y; 

Results_yp(i,s,:)=Nodes(s).pred.y; 

Results_Yp(i,s,:,:)=Nodes(s).pred.Y; 

end 

%  Carry  out  track-to-track  association 

global  trackestl; 

global  trackestcovl; 

global  trackpredl; 

global  trackpredcovl; 

global  trackest2; 

global  trackestcov2; 

global  trackpred2; 

global  trackpredcov2; 

chi  =  chi2inv(alpha,dof); 

countest  =  0;  countpred  =  0; 

trackpredcov2=zeros(XSIZE,  XSIZE); 

trackpred2=zeros(XSIZE,  1); 

warning  off; 

%  carry  out  track-to-track  fusion  for  predicted  and  estimated  tracks 
for  s=  1 :  NUM_N ODE  S 

trackpredcov  =  inv(Nodes(s).pred.Y); 
trackpred  =  trackpredcov*Nodes(s).pred.y; 
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if  ~isinf(trackpredcov) 
countpred  =  countpred+1; 
end 
end 

if  countpred  ==  1 

for  s=l  :NUM_NODES 

trackpredcov  =  inv(Nodes(s).pred.Y); 
trackpred  =  trackpredcov*Nodes(s).pred.y; 
if  ~isinf(trackprcdcov) 

trackpredcov  1  =  inv(Nodes(s).pred.Y); 
trackpredl  =  trackpredcov  1  *Nodes(s).pred.y; 
end 
end 

elseif  countpred  ==  0 

trackpredcov  1  =  inf(XSIZE,  XSIZE); 
trackpredl  =  nan(XSIZE,l); 
elseif  countpred  >  1 
for  s=l  :NUM_NODES 
if  countpred  >  0 

trackpredcov  =  inv(Nodes(s).pred.Y); 
trackpred  =  trackpredcov*Nodes(s).pred.y; 
if  ~isinf(  trackpredcov) 

trackpredcov  1  =  inv(Nodes(s).pred.Y); 
trackpredl  =  trackpredcov  l*Nodes(s).pred.y; 
countpred  =  countpred  -  1 ; 
if  s+1  <=  NUM_NODES  &&  countpred  >  0 
for  q  =  s+1  :NUM_NODES 
trackpredcovv  =  inv(Nodes(q).pred.Y); 
trackpredv  =  trackpredcovv*Nodes(q).pred.y; 
if  ~isinf(  trackpredcovv) 

trackpredcov2  =  inv(Nodes(q).pred.Y); 
trackpred2  =  trackpredcov2*Nodes(q).pred.y; 
countpred  =  countpred  -  1 ; 
end 

if  countpred  >=  0 

tracksumpredcov  =  trackpredcov  1  +  trackpredcov2; 
trackpreddiff  =  trackpredl  -  trackpred2; 

if  real(abs(trackpreddiff*inv(tracksumpredcov)*trackpreddiff))  <= 
chi 


+... 


trackpredl  =  trackpredcov2*inv(tracksumpredcov)*trackpredl 


trackpredcov  1  *  inv(tracksumpredco  v)  *  trackpred2 ; 
trackpredcov  1 

trackpredcov  1  *  inv(tracksumpredcov)  *  trackpredco  v2 ; 

else 
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trackpredl  =  trackestl; 
trackpredcovl  =  trackestcovl; 
end 
end 
end 
end 
end 
end 
end 
end 

TrackPredResults_y(i,:)=trackpred  1 ; 
TrackPredResultsY  (i,: ,  :)=trackpredcov  1 ; 

for  s=l:NUM_NODES 

trackestcov  =  inv(Nodes(s).est.Y); 
trackest  =  trackestcov*Nodes(s).est.y; 
if  ~isinf(trackestcov) 
countest  =  countest+1; 
end 
end 

if  countest  ==  1 

for  s=l:NUM_NODES 

trackestcov  =  inv(Nodes(s).est.Y); 
trackest  =  trackestcov*Nodes(s).est.y; 
if  ~isinf(trackcstcov) 

trackestcovl  =  inv(Nodes(s).est.Y); 
trackestl  =  trackestcovl  *Nodes(s).est.y; 
end 
end 

elseif  countest  ==  0 

trackestcovl  =  inf(XSIZE,  XSIZE); 
trackestl  =  nan(XSIZE,l); 
elseif  countest  >  1 

for  s=  1  :NUM_NODES 
if  countest  >  0 

trackestcov  =  inv(Nodes(s).est.Y); 
trackest  =  trackcstcov*Nodcs(s).est.y; 
if  ~isinf(  trackestcov) 

trackestcovl  =  inv(Nodes(s).est.Y); 
trackestl  =  trackestcovl  *Nodes(s).est.y; 
countest  =  countest  -  1 ; 
if  s+1  <=  NUM_NODES  &&  countest  >  0 
for  q  =  s+1  :NUM_NODES 
trackestcovv  =  inv(Nodes(q).est.Y); 
trackestv  =  trackestcovv*  Nodes(q).cst.y; 


334 


if  ~isinf(trackestcovv) 

trackestcov2  =  inv(Nodes(q).est.Y); 
trackest2  =  trackestcov2*Nodes(q).est.y; 
countest  =  countest  -  1 ; 
end 

if  countest  >=  0 

tracksumestcov  =  trackestcovl  +  trackestcov2; 
trackestdiff  =  trackestl  -  trackest2; 

if  real(abs(trackestdiff  *inv(tracksumestcov)*trackestdiff))  <=  chi 
trackestl  =  trackestcov2*inv(tracksumestcov)*trackestl  +... 

trackestco  v  1  *  inv(tracksumestco  v)  *  trackest2 ; 
trackestcovl  =  trackestcovl  *inv(tracksumestcov)*trackestcov2; 
end 
end 
end 
end 
end 
end 
end 
end 

TrackEstResults_y(i,  :)=trackest  1 ; 

TrackEstResultsY  (i, : ,  :)=trackestcov  1 ; 

P=trackestcovl; 
if  P  ~=  inf 

%  Calculate  the  position  uncertainty  of  the  target 

%  at  present  location 

x=trackestl; 

sigma=sqrt(P); 

xuncert  =  sigma(l,l); 

yuncert  =  sigma(3,3); 

distuncert  =  sqrt(xuncertA2  +  yuncertA2); 

b  =  circle([x(l),  x(3)], distuncert); 

%  Project  the  position  uncertainty  of  the  target 

%  at  final  destination 

FF=[1  (ntimes-i)*DT  0  0; 

0  1  0  0; 

0  0  1  (ntimes-i)*DT; 

0  0  0  1]; 

GG=[1  0  0  0; 

0  100; 

00  10; 

0  00  1]; 
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QQ=  [((ntimes-i)*DT)A2*SIGMA_XDOTA2  0  0  0; 

0  SIGMA_XDOTA2  0  0; 

0  0  ((ntimes-i)*DT)A2*SIGMA_YDOTA2  0; 

0  0  0  SIGMA_YDOTA2] ; 

MM=(inv(FF))'  *  (inv(P))*  (inv(FF)); 

SSigma=GG’*MM*GG+inv(QQ); 

OOmega=MM  *  GG  *  inv(S  Sigma); 

projYpred=MM-(OOmega*SSigma*OOmega'); 

proj  Ypred=force_sym(proj  Ypred); 

projP  =  inv(proj  Ypred); 

projPsigma  =  sqrt(projP); 

projxuncert  =  projPsigma(  1,1); 

projyuncert  =  projPsigma(3,3); 

projdistuncert  =  sqrt(projxuncertA2  +  projyuncertA2); 

projypred=(eye(4,4)-(OOmega*GG'))*(inv(FF))'*(inv(P)*x); 

projstate=  proj  P*proj  ypred; 

%  Check  if  SAW  has  entered  Port  Limits 
if  min(inpolygon  (b.X,  b.Y,  TSS_X,  TSS_Y))==  0  &&... 
min(inpolygon  (b.X,  b.Y,  PL_X,  PL_Y))  ==  0 

buf=sprintf('Warning:  Entered  Port  Limits  at  Timestep=%d',i-1);  disp(buf); 

%  Check  if  SAW  is  turning  towards  Jurong  Island 
disttoreach  =  sqrt(  abs(x(l)-302.1 144444)A2+... 

abs(x(3)-347.77 1 6667)A2); 
speedtoreach  =  sqrt(x(2)A2+x(4)A2); 
timetoreach  =  disttoreach/speedtoreach; 

%  Calculate  the  time  target  left  Port  Limits 
leftPL(i)  =  MAX_TIME-timetoreach; 

%  Calculate  the  distance  between  the  projected  target  end  point 
%  and  Jurong  Island 

distapart  =  sqrt((projstate(l)  -  302.1 144444)A2+(projstate(3)  -  347.77 16667)A2); 

%  Threat  level  assessment 

%  Send  warning  if  target  is  within  0.5nm  to  Jurong  Island 
if  distapart  <  5  &&  MAX  TIME  -  (i-l)*DT  >=  0  " 
buf=sprintf('Warning:  Pleading  Towards  Jurong  Island.  Time  to  Impact  =  %.2f 
sec', timetoreach);  disp(buf); 

if  projdistuncert  >  distapart 

buf=sprintf(’Warning:  High  Confidence  of  Impact’ );disp(buf); 
startplot=i-l; 

leftPLntoJIHigh(i)  =  MAXTIME-timetoreach; 

else 
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buf=sprintf('Warning:  Moderate  Confidence  of  Impact' );disp(buf); 
startplot=i-l; 

leftPLntoJIModerate(i)  =  MAXTIME-timetoreach; 
end 
end 

%  Check  if  SAW  has  left  Traffic  Separation  Scheme 
elseif  min(inpolygon  (b.X,  b.Y,  TSS_X,  TSS_Y))==  0  &&... 
min(inpolygon  (b.X,  b.Y,  PL_X,  PL_Y))  ==  1 
buf=sprintf(’Warning:  Left  Traffic  Separation  Scheme  at  Timestep=%d',(i-1)); 
disp(buf); 

%  Check  if  SAW  is  turning  towards  Jurong  Island 
disttoreach  =  sqrt(  abs(x(l)-302.1 144444)A2+... 

abs(x(3)-347.7716667)A2); 
speedtoreach  =  sqrt(x(2)A2+x(4)A2); 
timetoreach  =  disttoreach/speedtoreach; 

%  Calculate  the  time  target  left  TSS 
leftTSS(i)  =  MAXTIME-timetoreach; 

%  Calculate  the  distance  between  the  projected  target 

%  end  pointand  Jurong  Island 

distapart  =  sqrt((projstate(l)  -  302.1 144444)A2  +  ... 

(projstate(3)  -  347.77 1 6667)A2); 

%  Threat  level  assessment 

%  Send  warning  if  target  is  within  0.5nm  to  Jurong  Island 
if  distapart  <  5  &&  MAX  TIME  -  (i-l)*DT  >=  0 
buf=sprintf(’Warning:  Heading  Towards  Jurong  Island.  Time  to  Impact  =  %.2f 
sec', timetoreach);  disp(buf); 

if  projdistuncert  >  distapart 

buf=sprintf(' Warning:  High  Confidence  of  Impact’);disp(buf); 
startplot=i-l; 

leftTSSntoJIHigh(i)  =  MAXTIME-timetoreach; 

else 

buf=sprintf(' Warning:  Moderate  Confidence  of  Impact' );disp(buf); 
startplot=i-l; 

leftTSSntoJIModerate(i)  =  MAXTIME-timetoreach; 
end 
end 
end 


kdata(l  ,i)=times(i); 
kdata(2,i)=projdistunccrt*  1 80; 
kdata(3,i)=distapart*  1 80; 
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kdata(4,i)=disttoreach*  1 80; 
kdata(5,i)=speedtoreach*  1 80; 
kdata(6 ,  i)=timetoreach ; 
kdata(7,i)=startplot; 
kdata(8,i)=projP(  1,1); 
kdata(9,i)=projP(3,3); 
kdata(  1 0,i)=proj  state(  1 ); 
kdata(l  l,i)=projstate(3); 

end 

end 

disp(’Plotting  Data...'); 

figure(20); 

elf; 

plot(kdata(l,:),(kdata(2,:)),’b',kdata(l,:),kdata(3,:),'g') 
legend(’Distance  Uncertainty’, ’Distance  Apart’); 
buf=sprintf(’Estimated  End  Point'); 
title(buf) 

xlim([min(kdata(7, 1 000:MAX_TIME/DT))*DT  MAX  TIME- 1  *DT]) 
axis  'auto  y' 

xlabel(’Simulated  Time  (s)') 
ylabel(’Distance  (m)') 

figure(21); 

elf; 

plot(kdata(l,:),kdata(4,:),’b’,kdata(l,:),kdata(5,:),'g',kdata(l,:),kdata(6,:),T) 
legend('Distance  to  Impact’,’Speed  to  Impact',  'Time  to  Impact'); 
buf=sprintf(’Projected  Distance,  Speed  and  Time  to  Impact’); 
title(buf) 

xlim( [min(kdata(7,1000:MAX_TIME/DT))*DT  MAX  TIME- 1  *DT]) 

axis  'auto  y' 

xlabel('Simulated  Time  (s)') 

%  Record  the  times  the  target  crossed  TSS,  PL,  &  corresponding  threat  level 
firstleftTSS  =  min(leftTSS(1000:MAX_TIME/DT)); 

TirstleftTSSntoJIHigh  =  min(leftTSSntoJIHigh(1000:MAX_TIME/DT)); 
firstleftTSSntoJIModerate  =  min(leftTSSntoJIModerate(1000:MAX_TIME/DT)); 
firstleftPL  =  min(leftPL(  1 000:MAX_TIME/DT)); 
firstleftPLntoJIHigh  =  min(leftPLntoJIHigh(1000:MAX_TIME/DT)); 

(I  rstlcftPLntoJ  I  Moderate  =  min(leftPLntoJIModerate(1000:MAX_TIME/DT)); 


time  =  0; 
for  i=l:ntimes 
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P=squeeze(T  rackEstResults_Y  (i, : , :)); 
if  P  ==  inf 
time  =  i; 
end 
end 

warning  on; 

post _ sim(targets,  observations,  platforms,  destinations); 

plot _ tracks(targets,Results_y,Results_Y,Results_yp,Results_Yp,... 

TrackEstResults_y,TrackEstResults_Y,TrackPredResults_y,... 
T  rackPredResultsY,  times) 

plot _ errors(targets,TrackEstResults_y,TrackEstResults_Y, . . . 

TrackPredResults_y,TrackPredResults_Y,  times,  2,  time) 

plot _ coms(platforms,com_net) 

%  This  file  defines  the  global  variables 


%  General  information 


global  TSS  X; 
global  TSS  Y; 
global  PL_X; 
global  PL  Y ; 
global  DT; 


%  Traffic  Separation  Scheme  x-coordinates 
%  Traffic  Separation  Scheme  y-coordinates 
%  Port  Limits  x-coordinates 
%  Port  Limits  y-coordinates 
%  simulation  time-step 


global  MAX  TIME;  %  maximum  simulation  time 


%  Target  definitions 

global  TARGET  TYPE;  %  defines  type  of  target  (xy  or  V  phi) 

global  NUM  TARGETS;  %  number  of  targets  to  be  simulated 


%  Platfonn  definitions 

global  PLATFORM;  %  Platform  data  [xO,  yO,  phiO,  vel] 
global  NUM  PLATFORMS;  %  number  of  platforms 
global  MAX_RANGE;  %  maximum  observable  range 
global  SIGMA  XDOT; 
global  SIGMA  YDOT; 

%  Destination  definitions 

global  DESTINATION;  %  Destination  data  [xO,  yO,  phiO,  vel] 
global  NUM_DESTINATIONS;%  number  of  destinations 


%  Communications  definitions 
global  NUM_NODES;  %  number  of  sensor  nodes 

global  NUM_CHANS;  %  number  of  channels  per  node 
global  FULL_CON;  %  connectivity 


%  State  and  Sensor  definitions 
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global  SIGMA  Q;  %  process  noise  standard  deviation  for  feature  model 
global  SIGMA_RANGE_MPA;  %  Range  observation  noise 
global  SIGMABEARINGMPA;  %  Bearing  observation  noise 
global  SIGMA_RANGE_MPAC;  %  Range  observation  noise 
global  SIGMABEARINGMPAC;  %  Bearing  observation  noise 
global  SIGMA_RANGE_PV;  %  Range  observation  noise 
global  SIGMABEARINGPV;  %  Bearing  observation  noise 
global  SIGMARANGEPCG;  %  Range  observation  noise 
global  SIGMA_BEARING_PCG;  %  Bearing  observation  noise 
global  RfilterMPA;  %  observation  noise  covariance  for  MPA 
global  RfilterMPAC;  %  observation  noise  covariance  for  MPAC 
global  RfilterPV;  %  observation  noise  covariance  for  PV 
global  RfilterPCG;  %  observation  noise  covariance  for  PCG 
global  F;  %  state  transition  equation 

global  XSIZE;  %  state  dimension 

global  ZSIZE;  %  observation  dimension 

%  Results  "database" 
global  kdata; 
global  esttimesigma; 
global  startplot; 
global  firstleftTSS; 
global  firstleftTSSntoJIHigh; 
global  firstleftTSSntoJIModerate; 
global  firstleftPL; 
global  UrstleftPLntoJIHigh; 
global  firstleftPLntoJIModerate; 

%  Track  association  test  parameters 

global  alpha;  %  level  of  confidence  for  track  association 
global  dof;  %  degrees  of  freedom  for  Chi-statistics 

%  This  file  initializes  the  global  variables 

%  General  polygon  for  Traffic  Separation  Scheme  (TSS) 

X  =  [738.9629;  700.8405;  503.0354;  343.3085;  301.0754; 

291.7565;  356.6971;  474.1785;  703.9221;  738.9538;  738.9629]; 

Y  =  [443.8223;  412.1040;  377.3690;  303.7337;  330.3691; 

230.0498;  289.3964;  351.7903;  379.3511;  408.0001;  443.8223]; 

TSSX  =  X; 

TSSY  =  Y; 

%  General  polygon  for  Port  Limits,  estimated  0.5  mile  beyond  the  TSS 
[PL_X,  PL_Y]  =  bufferm2(’xy’,X,Y,5,’ouf); 


188.7756;  142.3798; 
359.0929;  316.1193; 
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MAX_TIME=1 1500;  %  simulation  duration 

DT=10;  %  simulation  time-step 

%  Target  definitions 

NUM_TARGETS=1 ;  %  number  of  targets  to  be  tracked 

%  Platform  definitions 
NUM_PLATFORMS=9; 

%  Destination  definitions 
NUM_DESTINATIONS=l; 

%  Number  of  sensor  nodes 
NUMN  ODE  S=NUM_PL  ATF  ORMS ; 

NUM_CHANS=0;  %  no  data  sharing  between  nodes 

FULL_CON=0;  %  no  connectivity  between  all  nodes 

%  State  and  Sensor  definitions 

SIGMA_RANGE_MPA=5;  %  0.5nm  range  error  equivalent 

SIGMA_BEARING_MPA=0.0525;  %  3  degrees  bearing  error  in  radians 
SIGMA_RANGE_MPAC=6;  %  0.6nm  range  error  equivalent 

SIGMA_BEARING_MPAC=0.0875;  %  5  degrees  bearing  error  in  radians 
SIGMA_RANGE_PV=3;  %  0.3nm  range  error  equivalent 

SIGMA_BEARING_PV=0.0525;  %  3  degrees  bearing  error  in  radians 
SIGMA_RANGE_PCG=4;  %  0.4nm  range  error  equivalent 

SIGMA_BEARING_PCG=0.0875;  %  5  degrees  bearing  error  in  radians 
Rfilter  MPA  =  [ SIGM A  RAN GE  MP AA2  0;0  SIGMA  RANGE  MP AA2] ; 
RfilterMPAC  =  [SIGMA_RANGE_MPACA2  0;0  S IGM  ARAN  GEMP  AC  A2  ] ; 
Rfilter  PV  =  [ S IGM A_RAN GE_P VA2  0;0  SIGMA  RANGE  P VA2] ; 

RfilterPCG  =  [  S  IGM  ARAN  GE_P  CG  A2  0;0  SIGMA_RANGE_PCGA2]; 
XSIZE=4;  %  number  of  state  components 

ZSIZE=2;  %  number  of  sensor  measurement  components 

SIGMA_XDOT=0. 00286;  %  std  dev  of  lknot  in  the  x-direction 

SIGMA_YDOT=0. 00286;  %  std  dev  of  lknot  in  the  y-direction 

%  Results  "database" 

kdata  =  [7  MAX  TIME/DT+ 1  ] ; 

%  Track  association  test  parameters 

alpha  =  0.99; 

dof  =  NUM_NODES; 

function  [latb,lonb]  =  bufferm2(varargin)  %lat, Ion, dist, direction, npts,outputformat) 
%BUFFERM2  Computes  buffer  zone  around  a  polygon 

% 

%  [latb,lonb]  =  bufferm2(lat, Ion, dist, direction) 
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% 

% 

% 

% 

% 

% 

% 

% 

% 

% 

% 

% 

% 

% 

% 
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% 

% 

% 

% 

% 

% 

% 

% 

% 

% 

% 

% 

% 

% 

% 

% 

% 

% 

% 

% 

% 


[latb,lonb]  =  bufferm2(lat,  Ion,  dist,  direction,  npts) 

[latb,lonb]  =  bufferm2(lat,  Ion,  dist,  direction,  npts,  outputformat) 

[xb,  yb]  =  bufferm2('xy',x,y, dist, direction, npts, outputformat) 

This  function  was  originally  designed  as  a  replacement  for  the  Mapping 
Toolbox  function  bufferm,  which  calculates  a  buffer  zone  around  a 
polygon.  The  original  bufferm  function  had  some  serious  bugs  that  could 
result  in  incorrect  buffer  results  and/or  errors,  and  was  also  very  slow. 

As  of  R2006b,  those  bugs  have  been  fixed.  However,  this  version  still 
maintains  a  few  advantages  over  the  original: 

-  Can  be  applied  to  polygons  in  either  geographical  space  (as  in 

bufferm)  or  in  cartesian  coordinates. 

-  Better  treatment  of  polygon  holes.  The  original  function  simply 

filled  in  all  holes;  this  version  trims  or  pads  holes  according  to  the 

buffer  width  given. 

Input  and  output  format  is  identical  to  buffenn  unless  the  'xy'  option  is 
specified,  so  it  can  be  used  interchangeably. 

Input  variables: 

lat:  Latitude  values  defining  the  polygon  to  be  buffered. 

This  can  be  either  a  NaN-delimited  vector,  or  a  cell 
array  containing  individual  polygonal  contours  (each  of 
which  is  a  vector).  External  contours  should  be  listed 
in  a  clockwise  direction,  and  internal  contours  (holes) 
in  a  counterclockwise  direction. 

Ion:  Longitude  values  defining  the  polygon  to  be  buffered. 

Same  format  as  lat. 

dist:  Width  of  buffer,  in  degrees  of  arc  along  the  surface 

(unless  'xy'  is  used,  in  which  case  units  correspond  to 
x-y  coordinates) 

direction:  'in’  or  'out' 

npts:  Number  of  points  used  to  contruct  the  circles  around 

each  polygon  vertex.  If  omitted,  default  is  13. 

outputformat:  'vector'  (NaN-delimited  vectors),  'cutvector' 

(NaN-clipped  vectors  with  cuts  connecting  holes  to  the 
exterior  of  the  polygon),  or  'cell'  (cell  arrays  in 
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%  which  each  element  of  the  cell  array  is  a  separate 

%  polygon),  defining  format  of  output.  If  omitted, 

%  default  is  'vector'. 

% 

%  'xy':  If  first  input  is  'xy',  then  data  will  be  assumed  to 

%  lie  on  a  cartesian  plane  rather  than  on  a  sphere.  Use 

%  x  and  y  coordinates  as  first  two  inputs  rather  than  lat 

%  and  Ion.  Units  of  x,  y,  and  distance  should  be  the 

%  same. 

% 

%  Output  variables: 

% 

%  lath:  Latitude  values  for  buffer  polygon 

% 

%  lonb:  Longitude  values  for  buffer  polygon 

% 

%  Example: 

% 

%  load  conus 

%  tol  =  0.1;  %  Tolerance  for  simplifying  polygon  outlines 
%  [reducedlat,  reducedlon]  =  reducem(gtlakelat,  gtlakelon,  tol); 

%  dist  =  1 ;  %  Buffer  distance  in  degrees 
%  [lath,  lonb]  =  bufferm2(reducedlat,  reducedlon,  dist,  'out'); 

%  figure('Renderer', ’painters') 

%  usamap({’MN’,'NY’}) 

%  geoshow(latb,  lonb,  'DisplayType',  'polygon',  'FaceColor',  'yellow') 
%  geoshow(gtlakelat,  gtlakelon,... 

%  'DisplayType',  'polygon',  'FaceColor',  'blue') 

%  geoshow(uslat,  uslon) 

%  geoshow(statelat,  statelon) 

% 

%  See  also: 

% 

%  bufferm,  polybool 
%  Copyright  2010  Kelly  Kearney 


% - 

%  Check  input 
% - 


error(nargchk(3 ,7,nargin)); 

%  Determine  if  geographic  or  cartesian 
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if  ischar(varargin{  1 })  &&  strcmp(varargin{  1 },  'xy') 
geo  =  false; 

param  =  varargin(2:end); 
else 

geo  =  true; 
param  =  varargin; 
end 

%  Set  defaults  if  not  provided  as  input 
nparam  =  length(param); 
if  geo 

[lat,  Ion,  dist]  =  deal(param{l:3}); 
else 

[Ion,  lat,  dist]  =  deal(param{  1:3});  %  Ion  =  x,  lat  =  y  for  mental  clarity,  will  switch 
back  at  end 
end 

if  nparam  <  4 
direction  =  'out'; 
else 

direction  =  param  {4}; 
end 

if  nparam  <  5 
npts  =  13; 
else 

npts  =  param{5}; 
end 

if  nparam  <  6 

outputformat  =  'vector'; 
else 

outputformat  =  param  {6}; 
end 

%  Check  fonnat  and  dimensions  of  input 

if  ~ismember(direction,  {'in',  'out'}) 

error('Direction  must  be  either  "in"  or  "out".'); 
end 

if  ~ismember(outputformat,  {'vector',  'cutvector',  'cell'}) 
error(’Unrecognized  output  format  flag.’); 
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end 


if -isnumeric(dist)  ||  numel(dist)  >  1 
error('Distance  must  be  a  scalar.') 
end 

if  ~isnumeric(npts)  ||  numel(npts)  >  1 

error('Number  of  points  must  be  a  scalar.') 
end 

if  iscell(lat) 

for  il  =  1  :numel(lat) 

if  ~isvector(lat{il})  ||  ~isvector(lon{il})  ||  ~isequal(length(lat{il}),  length(lon{il})) 
error('Lat  (or  x)  and  Ion  (or  y)  must  be  vectors  or  cells  of  vectors  with  identical 
dimensions'); 
end 

lat{il}  =  lat  {il}  (:); 
lon{il}  =  lon{il}(:); 
end 
else 

if -isvector(lat)  ||  -isvector(lon)  ||  ~iscqual(lcngth(lat),  length(lon)) 

error('Lat  (or  x)  and  Ion  (or  y)  must  be  vectors  or  cells  of  vectors  with  identical 
dimensions’); 
end 

lat  =  lat(:); 

Ion  =  lon(:); 
end 


%■ 


%  Split  polygon(s)  into 
%  separate  faces 


%■ 


if  iscell(lat) 

[lat,  Ion]  =  polyjoin(lat,  Ion);  %  In  case  multiple  faces  in  one  cell, 
end 

[latcells,  loncells]  =  polysplit(lat,  Ion); 


% - 

%  Create  buffer  shapes 
% - 


plotflag  =  0; 
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if  plotflag 

Plt.x  =  Ion; 

Plt.y  =  lat; 

end 

latcrall  =  cell(O); 
loncrall  =  cell(O); 

foripoly=  l:length(latcells) 

%  Circles  around  each  vertex 

if  geo 

[late,  lone]  =  calccircgeo(latcells{ipoly},  loncells{ipoly},  dist,  npts); 
else 

[lone,  late]  =  calccirccart(loncells{ipoly},  latcells {ipoly} ,  dist,  npts); 
end 

%  Rectangles  around  each  edge 
if  geo 

[latr,  lonr]  =  calcrecgeo(latcells{ ipoly},  loncells{  ipoly},  dist); 
else 

[lonr,  latr]  =  calcreccart(loncells  {ipoly},  latcells  {ipoly},  dist); 
end 

%  Union  of  circles  and  rectangles 

if  plotflag 

Plt.rectx  =  lonr; 

Plt.recty  =  latr; 

Plt.circx  =  lone; 

Plt.circy  =  late; 
end 

[late,  lone]  =  multipolyunion(latc,  lone); 

[latr,  lonr]  =  multipolyunion(latr,  lonr); 

if  plotflag 

Plt.rectcombox  =  lonr; 

Plt.rectcomboy  =  latr; 

Plt.circcombox  =  lone; 

Plt.circcomboy  =  late; 
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end 


[loner,  later]  =  polybool(’+',  lonr,  latr,  lone,  late); 

%  Union  of  new  circle/rectangle  combo  with  that  from  other  faces 

[loncrall,  latcrall]  =  polybool('+’,  loncrall,  latcrall,  loner,  later); 

%  Plotting  (for  debugging  only) 

if  plotflag 

Plt.allx  =  loncrall; 

Pit.  ally  =  latcrall; 

if  ipoly  ==  1 
figure; 

plot(Plt.x,  Plt.y,  ’k’,  ’linewidth',  2); 
hold  on 
end 

plot(cat(2,  Plt.rectxj:}),  cat(2,  Plt.recty{:}),  ’b’); 
plot(cat(2,  Plt.circx):}),  cat(2,  Plt.circy]:}),  'r'); 
plot(Plt.allx  { 1 } ,  Plt.ally { 1 } ,  'g',  'linewidth',  2); 

end 


% - 

%  Calculate  union/difference 
% - 


switch  direction 
case  ’out’ 

[lonb,  latb]  =  polybool('+’,  loncells,  latcells,  loncrall,  latcrall); 
case  ’in’ 

[lonb,  latb]  =  polybool(’-’,  loncells,  latcells,  loncrall,  latcrall); 
end 


if  plotflag 

[Plt.yfinal,  Plt.xfinal]  =  polyjoin(latb,  lonb); 

plot(Plt.xfinal,  Plt.yfinal,  ’linestyle', 'color',  [0  .5  0],  ’linewidth’,  2); 
end 
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% - 

%  Reformat  output 
% - 


if  ~geo 

y  =  latb;  %  Switch,  since  cartesion  uses  opposite  order 
x  =  lonb; 
latb  =  x; 
lonb  =  y; 
end 

switch  outputformat 
case  'vector' 

[latb,  lonb]  =  polyjoin(latb,  lonb); 
case  'cutvector' 

[latb,  lonb]  =  polycut(latb,  lonb); 

case  'cell' 
end 


***  * 

function  [late,  lone]  =  calccircgeo(lat,  Ion,  radius,  npts) 

%  lat  and  Ion:  n  x  1  vectors 
%  radius:  scalar 

radius  =  ones(length(lat),l)  *  radius; 

[late,  lone]  =  scirclel(lat,  Ion,  radius,  [],[],[],  npts); 
late  =  num2cell(latc,  1); 
lone  =  num2cell(lonc,  1); 

function  [latr,  lonr]  =  calcrecgeo(lat,  Ion,  halfwidth) 

%  lat  and  Ion:  n  x  1  vectors 
%  halfwidth:  scalar 

range  =  halfwidth  *  ones(length(lat)-l,  1); 

az  =  azimuth(lat(l:end-l),  lon(l:end-l),  lat(2:end),  lon(2:end)); 

[latbl  1  ,lonbl  1  ]  =  reckon(lat(l:end-l),  lon(l:end-l),  range,  az-90); 
[latbrl,lonbrl]  =  reckon(lat(l:end-l),  lon(l:end-l),  range,  az+90); 
[latbl2,lonbl2]  =  reckon(lat(2:end),  lon(2:end),  range,  az-90); 
[latbr2,lonbr2]  =  reckon(lat(2:end),  lon(2:end),  range,  az+90); 
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latr  =  [latbll  latbl2  latbr2  latbrl  latbl  1  ]'; 
lonr=  [lonbll  lonbl2  lonbr2  lonbrl  lonbll]'; 
latr  =  num2cell(latr,  1); 
lonr  =  num2cell(lonr,  1); 

function  [latu,  lonu]  =  multipolyunion(lat,  Ion) 

%  lat  and  Ion  are  n  x  1  cell  arrays  of  vectors 

latu  =  lat  { 1 } ; 
lonu  =  lon{  1 } ; 

for  ip  =  2:length(lat) 

[lonu,  latu]  =  polybool(’+',  lonu,  latu,  Ion  {ip},  lat  {ip}); 
end 

[latu,  lonu]  =  polysplit(latu,  lonu); 


function  [xc,  ye]  =  calccirccart(x,  y,  radius,  npts) 

ang  =  linspaee(0,  2*pi,  npts+1); 

ang  =  ang(end-l:-l:l); 

xc  =  bsxfun(@plus,  x,  radius  *  cos(ang)); 

ye  =  bsxfun(@plus,  y,  radius  *  sin(ang)); 

xc  =  num2cell(xc',  1); 

yc  =  num2cell(yc',  1); 

%  if  ~ispolycw(x,y) 

%  [xc,yc]  =  poly2ccw(xc,yc); 

%  end 

function  [xrec,  yrec]  =  calcreccart(x,  y,  halfwidth) 

dx  =  diff(x); 
dy  =  diff(y); 

isl  =  dx  >=  0  &  dy  >=  0; 
is2  =  dx  <  0  &  dy  >=  0; 
is3  =  dx  <  0  &  dy  <  0; 
is4  =  dx  >=  0  &  dy  <  0; 


ishl  =  dy  ==  0  &  dx  >  0; 
ish2  =  dy  ==  0  &  dx  <  0; 


theta  =  zeros(5,l); 
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theta(isl  |  is3)  =  atan(dy(isl  |  is3)./dx(isl  |  is3)); 
theta(is2  |  is4)  =  -atan(dy(is2  |  is4)./dx(is2  |  is4)); 

[xl,xr,yl,yr]  =  deal(zeros(size(dx))); 

xl(isl)  =  -halfwidth  *  sin(theta(isl)); 
xr(isl)  =  halfwidth  *  sin(theta(isl)); 
yl(isl)  =  halfwidth  *  cos(theta(isl)); 
yr(isl)  =  -halfwidth  *  cos(theta(isl)); 

xl(is2)  =  -halfwidth  *  sin(theta(is2)); 
xr(is2)  =  halfwidth  *  sin(theta(is2)); 
yl(is2)  =  -halfwidth  *  cos(theta(is2)); 
yr(is2)  =  halfwidth  *  cos(theta(is2)); 

xl(is3)  =  halfwidth  *  sin(theta(is3)); 
xr(is3)  =  -halfwidth  *  sin(theta(is3)); 
yl(is3)  =  -halfwidth  *  cos(theta(is3)); 
yr(is3)  =  halfwidth  *  cos(theta(is3)); 

xl(is4)  =  halfwidth  *  sin(theta(is4)); 
xr(is4)  =  -halfwidth  *  sin(theta(is4)); 
yl(is4)  =  halfwidth  *  cos(theta(is4)); 
yr(is4)  =  -halfwidth  *  cos(theta(is4)); 

xrec  =  [xl+x(l:end-l)  xl+x(2:end)  xr+x(2:end)  xr+x(l:end-l)  xl+x(l:end-l)]; 
yrec  =  [yl+y(l:end-l)  yl+y(2:end)  yr+y(2:end)  yr+y(l:end-l)  yl+y(l:end-l)]; 

xrec  =  num2cell(xrec,  2); 
yrec  =  num2cell(yrec,  2); 
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%  This  function  draws  a  circle  for  a  given  center  and  radius 
function  bubble  =  circle(center, radius) 

Resolution  =  100; 

THETA=linspace(0,2*pi, Resolution); 

RHO=ones(l  ,Resolution)*radius; 

[X,Y]  =  pol2cart(THETA,RHO); 
bubble.X=X+center(l); 
bubble.  Y=Y+center(2); 
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%  This  function  generates  the  SAW's  intended  destinations 
%  (i.e.  Areas  Al,  A2,  and  A3  of  Jurong  Island) 

function  destinations  =  generate _ destinations(times) 

%  Define  the  global  variables 
globals; 

%  Define  the  destination  locations 
for  i=l  :NUM_DESTINATIONS 
%  Area  Al 
if  i==l 

destinations(  1  ,i)  =  makedestination; 
destinations(  1  ,i)  .time=times(  1 ); 
destinations(  1  ,i)-id=i; 
destinations(l,i)-x  =  302.1 144444; 
destinations(l,i)-y  =  347.7716667; 

%  Now  iterate  for  all  times 
[temp,ntimes]=size(times); 
for  n=2:ntimes 

destinations(n,i)=destination_step(destinations(n-l,i),thnes(n)); 

end 

end 

%  Area  A2 
if  i==2 

destinations(  1  ,i)  =  makedestination; 
destinations(  1  ,i)  .time=times(  1 ); 
destinations(  1  ,i)-id=i; 
destinations(l,i)-x  =  322.731 1  111; 
destinations(l,i)-y  =  367.21 16667; 

%  Now  iterate  for  all  times 
[temp  ,ntimes] =size(times) ; 
for  n=2:ntimes 

destinations(n,i)=dcstination_stcp(dcstinations(n- 1  ,i),timcs(n)); 
end 
end 

%  Area  A3 
if  i==3 

destinations(  1  ,i)  =  makedestination; 
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destinations(  1  ,i)  .time=times(  1 ); 
destinations(l,i).id=i; 

destinations(l,i).x  =  343.3450000; 
destinations(l,i).y  =  382.5572222; 

%  Now  iterate  for  all  times 
[temp  ,ntimes] =size(times) ; 
for  n=2:ntimes 

destinations(n,i)=destination_step(destinations(n- 1  ,i),timcs(n)); 
end 
end 
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%  This  function  initialises  a  new  destination 


function  new=make_destination 

new=struct('id',0, 'time', 0,'x',0,'y',0, 'phi', 0,'vel',0, ’gamma’, 0); 
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%  This  function  is  to  step  a  destination 

function  next_destination=destination_step(destination,time) 
globals; 

%  This  is  a  no  motion  model 
dt=time-destination.time; 

nextdestination  =  makedestination;  %  space  for  destination  data  structure 

next_destination.id=destination.id; 

nextdestination.time  =  time; 

nextdestination.x  =  destination.x; 

nextdestination.y  =  destination.y; 

nextdestination.phi  =  destination.phi; 

nextdestination.vel  =  destination.vel; 

nextdestination.gamma  =  destination,  gamma; 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  generates  true  target  information  for  later  observation 
%  and  tracking.  Dynamics  of  target  are  contained  in  "target  step",  number 
%  targets  is  defined  by  the  globally  defined  variable  NUM_TARGETS.  A  data 
%  point  is  generated  at  each  of  a  set  of  "times.” 

function  targets  =  generate _ targets(times) 

%  Define  the  global  variables 
globals; 

%  Define  a  SAW  at  pre-fixed  location  in  the  TSS 
for  i=l 

targets(l,i)  =  make_target; 
targets(  1  ,i).time=times(  1 ); 
targets(l,i).id=i; 
targets(l,i).x  =  738.9616667; 
targets(l,i).y  =  438.7050000; 
targets(  1 ,  i) .  gamma=0 ; 

%  Now  iterate  for  all  times 
[temp,ntimes]=size(times); 
for  n=2:ntimes 

targets(n,i)=target_step_SAW_path(targets(n- 1  ,i),times(n)); 
end 
end 

%  Create  non-SAW  contacts  at  random  locations  within  the  TSS 
%  Not  used  since  this  thesis  covers  a  single  target 
for  i=2:NUM_TARGETS 
targets(l,i)  =  make_target; 
targets(  1  ,i).time=times(  1 ); 
targets(l,i).id=i; 

x  =  TSS_X(l)*rand; 
y  =  TSS_Y(l)*rand; 

while  (inpolygon  (x,  y,TSS_X,  TSS_Y)  ~=  1) 
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x  =  TSS_X(l)*rand; 
y  =  TSS_Y(l)*rand; 
end 

targets(l,i)-x  =  x; 
targets(l,i).y  =  y; 
targets(l,i)-phi  =  2*pi*rand  -pi; 

targets(l,i).vel  =  MINTARGETVEL  +  (M  AX_T  ARGETVEL- 

MIN_TARGET_VEL)*rand; 
targets(l  ,i)-gamma=0; 

%  Now  iterate  for  all  time 
[temp,ntimes]=size(times); 
for  n=2:ntimes 

targets(n,i)=target_step(targets(n- 1  ,i),timcs(n)); 
end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  This  function  initialises  a  new  target 

function  new=make_target 

new=struct('id',0, 'time', 0,'x',0,'yC0, 'phi', 0,'vel',0, 'gamma', 0); 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  is  the  trajectory  taken  by  the  SAW 

function  next_target=target_step_SAW_path(target,time) 

globals; 

dt=time-target.time; 
nexttarget  =  maketarget; 
next_target.id=target.id; 
nexttarget.time  =  time; 

%  target.vel  =  0.042870370  (in  m/s  which  equates  to  15knots) 

%  target.vel  =  0.059160494  (in  m/s  which  equates  to  20knots) 

%  target.vel  =  0.060018519  (in  m/s  which  equates  to  21  knots) 

%  target.vel  =  0.062876543  (in  m/s  which  equates  to  22knots) 

if  (next  target.time  >=  0  &&  next  target.time  <  1086) 
next_target.x  =  target.x  +  dt*0.042870370*cos(-pi+0.720); 
next_target.y  =  target. y  +  dt*0.042870370*sin(-pi+0.720); 
next_target.phi  =  -pi+0.720; 
next_target.vel  =  0.042870370; 

elseif  (next_target.time  >=  1086  &&  next_target.time  <  5890) 
next_target.x  =  target.x  +  dt*0.042870370*cos(-pi+0.170); 
next_target.y  =  target.y  +  dt*0.042870370*sin(-pi+0.170); 
next_target.phi  =  -pi+0.170; 
next_target.vel  =  0.042870370; 

elseif  (next  target.time  >=  5890  &&  next  target.time  <  10199) 
next_target.x  =  target.x  +  dt*0.042870370*cos(-pi+0.430); 
next_target.y  =  target.y  +  dt*0.042870370*sin(-pi+0.430); 
next_target.phi  =  -pi+0.430; 
next_target.vel  =  0.042870370; 

elseif  (next  target.time  >=  10199  &&  next  target.time  <  1 1051) 
next_target.x  =  target.x  +  dt*0.042870370*cos(pi-0.770); 
next_target.y  =  target.y  +  dt*0.042870370*sin(pi-0.770); 
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next_target.phi  =  pi-0.770; 
next_target.vel  =  0.042870370; 

elseif  (next  target.time  >=  11051  &&  nexttarget.time  <  11500) 
nexttarget.x  =  target. x  +  dt*0. 059 160494*cos(pi- 1.360); 
next_target.y  =  target.y  +  dt*0. 059 160494*sin(pi- 1.360); 
nexttarget.phi  =  pi-1.360; 
nexttarget.vel  =  0.059160494; 

elseif  (next_target.time  >=  11500) 
next_target.x  =  target.x; 
next_target.y  =  target.y; 
nexttarget.phi  =  0; 
next_target.vel  =  0; 

end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  generates  trajectories  for  platforms 

function  platforms  =  generate _ platforms(times) 

%  Define  the  global  variables 
globals; 

%  Define  5  fixed  platform  initial  locations 

%  Model  the  MPA  radar  stations 
for  i=l  :NUM_PLATFORMS 
if  i==l 

platforms(  l,i)  =  make_platform; 
platforms!  1  ,i).time=times(  1 ); 
platforms!  l,i).id=i; 
platforms!  l,i).x  =  285.4700000; 
platforms!  l,i).y  =  360.8527778; 
platforms! l,i).phi  =  2*pi*rand  -pi; 
platforms!  l,i).vel  =  0; 
platforms!  l,i).gamma=0; 

%  Now  iterate  for  all  time 
[temp,ntimes]=size(times); 
for  n=2:ntimes 

platforms(n,i)=platform_step(platforms(n- 1  ,i),times(n)); 
end 
end 

if  i==2 

platforms!  l,i)  =  make_platform; 
platforms!  1  ,i).time=times(  1 ); 
platforms!  l,i).id=i; 
platforms!  l,i).x  =  342.7972222; 
platforms!  l,i).y  =  311.9850000; 
platforms! l,i).phi  =  2*pi*rand  -pi; 
platforms!  l,i).vel  =  0; 
platforms!  l,i).gamma=0; 
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%  Now  iterate  for  all  time 
[temp,ntimes]=size(times); 
for  n=2:ntimes 

platforms(n,i)=platform_step(platforms(n- 1  ,i),times(n)); 
end 
end 

if  i==3 

platforms(l,i)  =  make_platform; 
platforms(l,i)-time=times(l); 
platforms(l,i)-id=i; 
platforms(l,i).x  =  413.1333333; 
platforms(l,i).y  =  349.8194444; 
platforms(l,i)-phi  =  2*pi*rand  -pi; 

platforms(l,i)-vel  =  0; 

platforms(l,i)-gamma=0; 

%  Now  iterate  for  all  time 
[temp  ,ntimes] =size(times) ; 
for  n=2:ntimes 

platforms(n,i)=platfonn_step(platfonns(n- 1 ,  i),ti  mcs(  n )); 
end 
end 

if  i==4 

platforms(l,i)  =  make_platform; 
platforms(  1  ,i).time=times(  1 ); 
platforms(  1  ,i)-id=i; 
platforms(  1  ,i).x  =  46 1 .476 1 1 1 1 ; 
platforms(  1  ,i).y  =  403 .426 1111; 
platforms(l,i)-phi  =  2*pi*rand  -pi; 
platforms(l,i)-vel  =  0; 
platforms(  1  ,i)-gamma=0; 

%  Now  iterate  for  all  time 
[temp  ,ntimes] =size(times) ; 
for  n=2:ntimes 

platforms(n,i)=platfonn_step(platforms(n- 1  ,i),timcs(n)); 
end 
end 

if  i==5 

platforms(l,i)  =  make_platform; 
platforms(l,i)-time=times(l); 
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platforms(l,i)-id=i; 
platforms(l,i)-x  =  517.0844444; 
platforms(l,i).y  =  421.9305556; 
platforms(l,i)-phi  =  2*pi*rand  -pi; 
platforms(l,i)-vel  =  0; 
platforms(  1  ,i)-gamma=0; 

%  Now  iterate  for  all  time 
[temp,ntimes]=size(times); 
for  n=2:ntimes 

platforms(n,i)=platfomi_step(platforms(n- 1 ,  i),ti  mes(  n )); 
end 
end 

%  Define  3  mobile  platform  initial  locations 

%  Model  the  CPC 
if  i==6 

platforms(l,i)  =  make_platform; 
platforms(  1  ,i).time=times(  1 ); 
platforms(  1  ,i)-id=i; 
platforms(l,i).x  =  474.1785056; 
platforms(l,i).y  =  351.7903000; 

platforms(l,i)-phi  =  2*pi*rand  -pi; 
platforms(l,i)-vel  =  0; 

targets(  1 ,  i) .  gamma=0 ; 

%  Now  iterate  for  all  time 
[temp  ,ntimes] =size(times) ; 
for  n=2:ntimes 

platforms(n,i)=platfonn_step_PCG  l_path(platforms(n- 1  ,i),timcs(n)); 
end 
end 

if  i— 7 

platforms(l,i)  =  make_platform; 
platforms(l,i)-time=times(l); 
platforms(  1  ,i).id=i; 
platforms(l,i).x  =  474.1785056; 
platforms(l,i).y  =  351.7903000; 
platforms(l,i)-phi  =  2*pi*rand  -pi; 
platforms(l,i)-vel  =  0; 
targets(  1  ,i)  •  gamma=0 ; 

%  Now  iterate  for  all  time 
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[temp,ntimes]=size(times); 
for  n=2:ntimes 

platforms(n,i)=platfomi_step_PCG2_path(platforais(n-l,i),times(n)); 

end 

end 

%  Model  the  PV 
if  i==8 

platforms(l,i)  =  make_platform; 
platforms(  1  ,i).time=times(  1 ); 
platforms!  1  ,i).id=i; 
platforms!  l,i).x  =  474.1785056; 
platforms!  l,i).y  =  351.7903000; 
platforms! l,i)-phi  =  2*pi*rand  -pi; 
platforms!  l,i)-vel  =  0; 
targets!  1  ,i)  •  gamma=0 ; 

%  Now  iterate  for  all  time 
[temp,ntimes]=size(times); 
for  n=2:ntimes 

platforms!n,i)=platform_stcp_PV_path(platforms(n-l  ,i),timcs!n)); 
end 
end 

%  Given  the  speed  and  sensor  coverage  of  the  MPaA, 

%  its  surveying  of  the  small  TSS  can  be  modeled  as  a  "fixed"  sensor 
%  with  complete  of  the  TSS  during  its  operation. 

%  Model  the  MPaA 
if  i==9 

platforms!  1  ,i)  =  make_platform; 
platforms!  1  ,i)  .time=times(  1 ); 
platforms!  1  ,i)-id=i; 
platforms!  l,i).x  =  474.1785056; 
platforms!  l,i).y  =  351.7903000; 
platforms! l,i)-phi  =  2*pi*rand  -pi; 
platforms!  l,i)-vel  =  0; 
targets!  1  ,i)-gamma=0; 

%  Now  iterate  for  all  time 
[temp,ntimes]=size(times); 
for  n=2:ntimes 

platforms(n,i)=platform_step_MPAC(platforms(n-l  ,i),times!n)); 
end 
end 
end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  This  function  initialises  a  new  platfonu 

function  new=make_platform 

new=struct('id',0, 'time', 0,'x',0,'yC0, 'phi', 0,'vel',0, 'gamma', 0); 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  This  is  a  no  motion  platform  model  for  MPA  radar  station 

function  next_platform=platform_step(platform,time) 

globals; 

dt=time-plat  form,  time; 
next_platform  =  make_platfonn; 
next_platform.id=platform.id; 
next_platform.time  =  time; 
next_platform.x  =  platfonn.x; 
next_platform.y  =  platform.y; 
next_platform.phi  =  platform.phi; 
next_platform.vel  =  platform,  vel; 
next_platform. gamma  =  platform.gamma; 
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%  This  is  the  platform  motion  model  for  one  of  the  two  CPCs  for  a  day 
function  next_platfonn=platform_step_PCG  l_path(platfonn,time) 
globals; 

dt=time-platform.time; 
next_platfonn  =  make_platfonn; 
next_platfonn.id=platform.id; 
next_platfonn.time  =  time; 

if  (next_platform.time  >=  0) 
next_platfonn.x  =  474.1785056; 
next_platfonn.y  =  351.7903000; 
next_platfonn.phi  =  platform.phi; 
next_platfonn.vel  =  platform.vel; 
next_platfonn.  gamma  =  platform. gamma; 
end 


% - 

%  This  models  the  platform's  patrol  path.  However,  by  making  the  assumption 
%  that  the  sensors  have  extended  range  to  simplify  sensor  management,  this 
%  modeling  is  not  used  here.  This  will  be  useful  for  future  work  with  the 
%  inclusion  of  sensor  management. 

% 

%  if  (next_platfonn.time  >=  0  &&  next_platfonn.time  <  18000) 

%  next_platform.x  =  platform.x  +  dt*0.015061617*cos(l.  519574188-0. 5*pi); 
%  next_platfonn.y  =  platform.y  +  dt*0.015061617*sin(l.  519574188-0. 5*pi); 
%  next_platfonn.phi  =  1. 519574188-0. 5*pi; 

%  next_platfonn.vel  =  0.015061617; 

%  next_platfonn. gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  18000  &&  next_platform.time  <  32400) 

%  next_platform.x  =  platform.x  +  dt*0.025102695*cos(l.  519574188+0. 5*pi); 
%  next_platfonn.y  =  platform.y  +  dt*0.025102695*sin(l.  519574188+0. 5*pi); 
%  next_platfonn.phi  =  1.5 19574188+0. 5*pi; 

%  next_platfonn.vel  =  0.025 102695; 

%  next_platform.  gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  32400  &&  next_platform.time  <  46800) 

%  next_platfonn.x  =  platform.x  +  dt*0.015061617*cos(l.  5 19574188-0. 5*pi); 
%  next_platform.y  =  platform.y  +  dt*0.0 1506 1 6 1 7*sin(  1 . 519574188-0. 5*pi); 
%  %next_platform.phi  =  1. 519574188-0. 5*pi; 

%  %next_platform.vel  =  0.015061617; 
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%  %next_platform. gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  46800  &&  next_platform.time  <  61200) 

%  next_platform.x  =  platform.x  +  dt*0.025102695*cos(l.  519574188+0. 5*pi); 
%  next_platform.y  =  platform.y  +  dt*0.025102695*sin(l.  519574188+0. 5*pi); 
%  next_platform.phi  =  1. 519574188+0. 5*pi; 

%  next_platform.vel  =  0.025 102695; 

%  next_platform.  gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  61200  &&  next_platform.time  <  75600) 

%  next_platfonn.x  =  platform.x  +  dt*0.015061617*cos(l. 5 19574188-0. 5*pi); 
%  next_platform.y  =  platform.y  +  dt*0.0 1 506 1 6 1 7*sin(  1 . 519574188-0. 5*pi); 
%  %next_platform.phi  =  1. 519574188-0. 5*pi; 

%  %next_platform.  vel  =  0.015061617; 

%  %next_platform. gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  75600  &&  next_platform.time  <  86400) 

%  next_platform.x  =  platform.x  +  dt*0. 025 102695*cos(l.  5 19574188+0. 5*pi); 
%  next_platfonn.y  =  platform.y  +  dt*0.025102695*sin(l. 519574188+0. 5*pi); 
%  next_platform.phi  =  1. 519574188+0. 5*pi; 

%  next_platfonn.vel  =  0.025 102695; 

%  next_platform.  gamma  =  platform,  gamma; 

% 

%  elseif  (next_platform.time  >=  86400) 

%  next_platfonn.x  =  platform.x; 

%  next_platform.y  =  platform.y; 

%  next_platform.phi  =  0; 

%  next_platform.vel  =  0; 

%  next_platform.  gamma  =  0; 

%  end 

% - 
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%  This  is  the  platform  motion  model  for  one  of  the  two  CPCs  for  a  day 
function  next_platform=platform_step_PCG2_path(platform,time) 
globals; 

dt=time-platform.time; 
next_platfonn  =  make_platfonn; 
next_platfonn.id=platform.id; 
next_platfonn.time  =  time; 

if  (next_platform.time  >=  0) 
next_platfonn.x  =  474.1785056; 
next_platfonn.y  =  351.7903000; 
next_platfonn.phi  =  platform.phi; 
next_platfonn.vel  =  platform.vel; 
next_platfonn.  gamma  =  platform. gamma; 
end 


% - 

%  This  models  the  platform's  patrol  path.  However,  by  making  the  assumption 
%  that  the  sensors  have  extended  range  to  simplify  sensor  management,  this 
%  modeling  is  not  used  here.  This  will  be  useful  for  future  work  with  the 
%  inclusion  of  sensor  management. 

% 

%  if  (next_platfonn.time  >=  0  &&  next_platform.time  <  18000) 

%  next_platform.x  =  platform.x  +  dt*0.013140537*cos(-l.  330818664+0. 5*pi); 
%  next_platfonn.y  =  platform.y  +  dt*0.013140537*sin(-l. 330818664+0. 5*pi); 
%  next_platfonn.phi  =  - 1.3308 18664+0. 5  *pi; 

%  next_platfonn.vel  =  0.013 140537; 

%  next_platfonn. gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  18000  &&  next_platform.time  <  32400) 

%  next_platform.x  =  platform.x  +  dt*0. 02 1900894*cos(- 1.3308 18664-0. 5  *pi); 
%  next_platfonn.y  =  platform.y  +  dt*0. 02 1900894*sin(- 1.3308 18664-0. 5  *pi); 
%  next_platfonn.phi  =  -1. 330818664-0. 5*pi; 

%  next_platfonn.vel  =  0.021900894; 

%  next_platform.  gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  32400  &&  next_platform.time  <  46800) 

%  next_platfonn.x  =  platform.x  +  dt*0.013140537*cos(-l. 330818664+0. 5*pi); 
%  next_platform.y  =  platform.y  +  dt*0.013140537*sin(-l. 330818664+0. 5  *pi); 
%  next_platform.phi  =  - 1.3308 18664+0. 5  *pi; 

%  next_platfonn.vel  =  0.013 140537; 
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%  next_platform.  gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  46800  &&  next_platform.time  <  61200) 

%  next_platform.x  =  platform.x  +  dt*0. 02 1900894*cos(- 1.3308 18664-0. 5  *pi); 
%  next_platform.y  =  platform.y  +  dt*0. 02 1900894*sin(- 1.3308 18664-0. 5  *pi); 
%  next_platform.phi  =  -1. 330818664-0. 5*pi; 

%  next_platform.vel  =  0.021900894; 

%  next_platform. gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  61200  &&  next_platform.time  <  75600) 

%  next_platform.x  =  platform.x  +  dt*0.013140537*cos(-l. 330818664+0. 5*pi); 
%  next_platform.y  =  platform.y  +  dt*0.013140537*sin(-l. 330818664+0. 5  *pi); 
%  next_platform.phi  =  - 1.3308 18664+0. 5  *pi; 

%  next_platfonn.vel  =  0.013 140537; 

%  next_platform.  gamma  =  platform,  gamma; 

% 

%  elseif  (next_platform.time  >=  75600  &&  next_platform.time  <  86400) 

%  next_platform.x  =  platform.x  +  dt*0.021900894*cos(-l. 330818664-0. 5*pi); 
%  next_platform.y  =  platform.y  +  dt*0. 02 1900894*sin(- 1.3308 18664-0. 5  *pi); 
%  next_platform.phi  =  -1. 330818664-0. 5*pi; 

%  next_platfonn.vel  =  0.02 1900894; 

%  next_platfonn.  gamma  =  platform,  gamma; 

% 

%  elseif  (next_platform.time  >=  86400) 

%  next_platform.x  =  platform.x; 

%  next_platform.y  =  platform.y; 

%  next_platform.phi  =  0; 

%  next_platform.vel  =  0; 

%  next_platform.  gamma  =  0; 

%  end 

% - 
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%  This  is  the  platform  motion  model  for  PV  for  a  day 
function  next_platform=platform_step_PV_path(platform,time) 
globals; 

dt=time-platform.time; 
next_platfonn  =  make_platfonn; 
next_platfonn.id=platform.id; 
next_platfonn.time  =  time; 

if  (next_platform.time  >=  0) 
next_platfonn.x  =  474.1785056; 
next_platfonn.y  =  351.7903000; 
next_platfonn.phi  =  platform.phi; 
next_platfonn.vel  =  platform.vel; 
next_platfonn.  gamma  =  platform. gamma; 
end 


% - 

%  This  models  the  platform's  patrol  path.  However,  by  making  the  assumption 
%  that  the  sensors  have  extended  range  to  simplify  sensor  management,  this 
%  modeling  is  not  used  here.  This  will  be  useful  for  future  work  with  the 
%  inclusion  of  sensor  management. 

% 

%  if  (next_platfonn.time  >=  0  &&  next_platform.time  <  43200) 

%  next_platform.x  =  platform.x  +  dt*0.013972407*cos(0. 1528 13367); 

%  next_platfonn.y  =  platform.y  +  dt*0.013972407*sin(0. 1528 13367); 

%  next_platfonn.phi  =  0.152813367; 

%  next_platfonn.vel  =  0.013972407; 

%  next_platfonn. gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  43200  &&  next_platfonn.time  <  86400) 

%  next_platform.x  =  platform.x  +  dt*0.013972407*cos(0.152813367+pi); 

%  next_platform.y  =  platform.y  +  dt*0.013972407*sin(0.152813367+pi); 

%  next_platform.phi  =  0.152813367+pi; 

%  next_platform.vel  =  0.013972407; 

%  next_platform.  gamma  =  platform. gamma; 

% 

%  elseif  (next_platform.time  >=  86400) 

%  next_platform.x  =  platform.x; 

%  next_platform.y  =  platform.y; 

%  next_platform.phi  =  0; 

%  next_platform.vel  =  0; 
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%  next_platform. gamma  =  0; 

%  end 

% - - - 
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%  This  is  the  platform  motion  model  for  MPaA 

function  next_platfonn=platform_step_MPAC(platfonn,time) 

globals; 

next_platfonn  =  make_platfonn; 
next_platfonn.id=platform.id; 
next_platfonn.time  =  time; 

if  (next_platform.time  >=  0) 
next_platfonn.x  =  474.1785056; 
next_platfonn.y  =  351.7903000; 
next_platfonn.phi  =  platform.phi; 
next_platfonn.vel  =  platform.vel; 
next_platfonn.  gamma  =  platform. gamma; 
end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  is  to  get  a  fixed  filter  parameter  block 

function  filter=get_filter_params 

globals; 

%  Assume  a  constant  velocity  model; 

filter.XDIM=4; 

filter.  ZD  IM=2; 

filter. Q=  [DTA2 * SIGM A_XDOTA2  0  0  0; 

0  SIGMA_XDOTA2  0  0; 

0  0  DTA2*SIGMA_YDOTA2  0; 

0  0  0  SIGMA_YDOTA2] ; 

filter. H=  [1  0  0  0;  0  0  1  0]; 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  Returns  a  sensor  data  structure  which  contains  all 
%  necessary  infonnation  for  a  decentralised  sensor. 

function  sensor=make _ sensor(id, filter) 

globals; 

ginit; 

%  Define  sensors  for  the  5  fixed  platforms 
if  id==l 
sensor.  id=id; 
sensor.time=0.0; 

sensor.r_err=SIGMA_RANGE_MPA; 
sensor.  b_err=SIGMA_BEARING_MPA; 
sensor.point=0;  %  zero  point  angle 
sensor.beam_view=2*pi; 

sensor.max_range=  154;  %  an  equivalent  of  1 5nm 
sensor.filter=filter; 

sensor.  est=make_track(id,  filter  .XDIM,  0.0); 
sensor.pred=make_track(id,filter.XDIM,0.0); 
sensor.  num_chans=NUM_CHAN  S ; 
for  i=l  :NUM_CHANS 

sensor.  chan_in(i)=make_track(i, filter  .XDIM, 0.0); 
sensor.  chan_out(i)=make_track(i, filter  .XDIM, 0.0); 
end 


elseif  id==2 
sensor.  id=id; 
sensor.thne=0.0; 

sensor.r_err=SIGMA_RANGE_MPA; 
sensor.  b_err=S  IGM  A_BE  ARIN  G_MP  A ; 
sensor.point=0; 
sensor.beam_view=2*pi; 
sensor.max_range=  154; 
sensor.filter=filter; 

sensor.  est=make_track(id,filter.XDIM, 0.0); 
sensor.pred=make_track(id,filter.XDIM,0.0); 
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sensor.  num_chans=NUM_CH  AN  S ; 
for  i=  1  :NUM_CHAN  S 

sensor.  chan_in(i)=make_track(i, filter  .XDIM, 0.0); 
sensor.chan_out(i)=make_track(i, filter  .XDIM, 0.0); 
end 


elseif  id==3 
sensor.  id=id; 
sensor.time=0.0; 

sensor.  r_err=S  IGM  A_RAN  GE_MP  A ; 
sensor.  b_err=S  IGM  ABE  ARIN  G_MP  A ; 
sensor.point=0; 
scnsor.beam_vicw=2*pi; 
sensor.max_range=  154; 
sensor.  filter=filter; 

sensor.  est=make_track(id, filter  .XDIM, 0.0); 
sensor.pred=make_traek(id,filter.XDIM,0.0); 
sensor.  num_chans=NUM_CH  AN  S ; 
for  i=  1  :NUM_CHAN S 

sensor.  chan_in(i)=make_track(i,  filter  .XDIM,  0.0); 
sensor.  chan_out(i)=make_track(i, filter  .XDIM, 0.0); 
end 


elseif  id==4 
sensor.  id=id; 
sensor.time=0.0; 

sensor.r_err=SIGMA_RANGE_MPA; 
sensor.  b_err=S  IGM  ABE  ARIN  G_MP  A ; 
sensor.point=0; 
sensor.beam_view=2*pi; 
sensor.max_range=  154; 
sensor.  filter=filter; 

sensor.  est=make_track(id,  filter  .XD IM,  0 . 0) ; 
sensor.pred=make_traek(id,filter.XDIM,0.0); 
sensor.  num_chans=NUM_CHAN  S ; 
for  i=l  :NUM_CHANS 

sensor.  chan_in(i)=make_track(i, filter  .XDIM, 0.0); 
sensor.  chan_out(i)=make_track(i, filter  .XDIM, 0.0); 
end 


elseif  id==5 
sensor.  id=id; 
sensor.time=0.0; 

sensor.r_err=SIGMA_RANGE_MPA; 
sensor.  b_err=SIGMA_BEARING_MPA; 
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sensor.point=0; 
sensor.beam_view=2*pi; 
sensor.max_range=  154; 
sensor.  filtcr=  filter; 

sensor.  est=make_track(id, filter  .XDIM, 0.0); 
sensor.pred=make_track(id,filter.XDIM,0.0); 
sensor.  num_chans=NUM_CHAN  S ; 
for  i=l  :NUM_CHANS 

sensor.  chan_in(i)=make _ track(i,  filter  .XDIM,  0.0); 

sensor.  chan_out(i)=make _ track(i, filter  .XDIM, 0.0); 

end 

%  Define  sensors  for  the  4  mobile  platforms 

%  Define  the  CPC  sensor 
elseif  id==6 
sensor.  id=id; 
sensor.time=0.0; 

sensor.r_err=SIGMA_RANGE_PCG; 
sensor.b_err=SIGMA_BEARING_PCG; 
sensor.point=0;  %  zero  point  angle 
sensor.beam_view=2*pi; 

sensor.max_range=1000;  %  range  extended  to  simulate 
%  presence  at  Jurong  Island 
sensor.filter=filter; 

sensor.  est=make_track(id,  filter  .XDIM,  0.0); 
sensor.pred=make_track(id,filter.XDIM,0.0); 
sensor.  num_chans=NUM_CHAN  S ; 
for  i=l  :NUM_CHANS 

sensor.  chan_in(i)=make_track(i, filter  .XDIM, 0.0); 
sensor.  chan_out(i)=make_track(i, filter  .XDIM, 0.0); 
end 

%  Define  the  CPC  sensor 
elseif  id==7 
sensor.  id=id; 
sensor.time=0.0; 

sensor.r_err=SIGMA_RANGE_PCG; 
sensor.  b_err=SIGMA_BEARING_PCG; 
sensor.point=0; 
sensor.beam_view=2*pi; 
sensor.max_range=  1 000; 
sensor.filter=filter; 

sensor.  est=make_track(id,filter.XDIM, 0.0); 
sensor.pred=make_track(id,filter.XDIM,0.0); 
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sensor.  num_chans=NUM_CH  AN  S ; 
for  i=  1  :NUM_CHAN S 

sensor.  chan_in(i)=make_track(i, filter  .XDIM, 0.0); 
sensor.chan_out(i)=make_track(i, filter  .XDIM, 0.0); 
end 

%  Define  the  PV  sensor 
elseif  id==8 
sensor.  id=id; 
sensor.time=0.0; 

sensor.r_err=SIGMA_RANGE_PV ; 
sensor.  b_err=SIGMA_BEARING_PV; 
sensor.point=0; 
sensor.beam_view=2*pi; 
sensor.max_range=  1 000; 
sensor.  filter=filter; 

sensor.  est=make_track(id, filter  .XDIM, 0.0); 
sensor.pred=make_traek(id,filter.XDIM,0.0); 
sensor.  num_chans=NUM_CH  AN  S ; 
for  i=  1  :NUM_CHAN S 

sensor.  chan_in(i)=make_track(i, filter  .XDIM, 0.0); 
sensor.  chan_out(i)=make_track(i, filter  .XDIM, 0.0); 
end 

%  Define  the  MPaA  sensor 
elseif  id==9 
sensor.  id=id; 
sensor.time=0.0; 

sensor.r_err=SIGMA_RANGE_MPAC; 
sensor.  b_err=S  IGM  ABE  ARIN  G_MP  AC ; 
sensor.point=0; 
sensor.beam_view=2*pi; 
sensor.max_range=  1 000; 
sensor.  filter=filter; 

sensor.  est=make_track(id, filter  .XDIM, 0.0); 
sensor.pred=make_traek(id,filter.XDIM,0.0); 
sensor.  num_chans=NUM_CH  AN  S ; 
for  i=  1  :NUM_CHAN S 

sensor.  chan_in(i)=make_track(i, filter  .XDIM, 0.0); 
sensor.  chan_out(i)=make_track(i, filter  .XDIM, 0.0); 
end 
end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  This  function  makes  space  for  a  track 

function  track=make_track(id, dim, time) 

track.  id=id; 
track.XDIM=dim; 
track.time=time; 
track.y=zeros(dim,  1 ); 
track.  Y=zeros(dim, dim); 


%  This  function  initialises  a  network  structure 

function  network=init_net(platforms, nodes) 

nsize=length(nodes); 

network=ones(nsize,nsize); 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  constructs  a  report  for  a  sensor  at  a  particular  time 

function  report=sensor _ report(sensor, platforms, targets, nsensor,ntime) 

globals; 

report=zeros(NUM_TARGETS,5); 

%  Find  platform  location  at  this  time 

px=platforms(ntime,nsensor).x; 

py=platforms(ntime,nsensor).y; 

for  tnum=l  :NUM_TARGETS 
%  Find  target  location 
tx=targets(ntime,tnum).x; 
ty=targets(ntime,tnum).y; 

%  Compute  true  range  and  bearing 

dx=tx-px; 

dy=ty-py; 

range=sqrt(dx*dx  +  dy*dy); 
bearing=atan2(dy,dx); 

%  determine  if  this  is  actually  visable 
if  range  <  sensor.max  range 
%  Add  error  from  sensor  model 
report(tnum,  1  )=tnum; 

report(tnum,2)=range  +  sensor.r_err*(-l+2*rand); 
report(tnum,3)=bearing  +  scnsor.b_err5|5(- 1  +2*rand); 
report(tnum,4)=px; 
report(tnum,  5  )=py ; 
else 

report(tnum,  1  )=tnum; 
report(tnum,2)=NaN; 
report(tnum,3)=NaN; 
report(tnum,4)=NaN; 
report(tnum,  5  )=N  aN ; 
end 
end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  performs  local  prediction  for  each  node  using  the  CVM 

function  node=local_predict(node,time) 

globals; 

dt=time-node .  time ; 
if  dt  <  0 

error(’Negative  prediction  step  in  state_pred’) 
end 

%  Compute  matrices 
F=[l  dt  0  0; 

0  100; 

0  0  1  dt; 

00  0  1]; 

G=[l  0  0  0; 

0  100; 

00  10; 

00  0  1]; 

Q=node.filter.Q; 

%  Do  prediction 

M=(inv(F))'  *  (node.  est.  Y)  *  (inv(F)); 

Sigma=G'*M*G+inv(Q); 

Omega=M  *  G  *  inv(  S  igma) ; 
node.pred.Y=M-(Omega*Sigma*Omega'); 
node.pred.y=(eye(4,4)-(Omega*G'))*(inv(F))'*(node.est.y); 
node.time=time; 

%  Seems  to  require  for  stability  in  Matlab 
node.pred.Y=force_sym(node.pred.Y); 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  forces  symmetry  on  a  matrix 

function  sigma=force_sym(sigma) 

[nx,ny]=size(sigma); 
if  nx  ~=  ny 

error('Matrix  must  be  square’); 
end 


for  i=l:nx 
for  j=i+l:nx 

temp=(sigma(i,j)+sigma(j,i))/2; 

sigma(i,j)=temp; 

sigma(j,i)=temp; 

end 

end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  perfonns  information  filter  local  update  for  each  node 

function  node=local_update(node,obs,i) 

globals; 

%  Observation  model 
H=node.filter.H; 

if  (isnan(obs.report(2:5)))  %  Wait  for  observation 
node.est.y=zeros(XSIZE,l); 
node.est.Y=zeros(XSIZE,  XSIZE); 
else  %  Update  with  observation 
%  Extract  observation 

[zx,zy,R]=xy_obs(obs. report, l,i);  %  R  is  not  used 

z=[zx;zy]; 

if  i==l 

Ri=inv(Rfiltcr_MPA); 
elseif  i==2 

Ri=inv(Rfilter_MPA); 
elseif  i==3 

Ri=inv(Rfilter_MPA); 
elseif  i==4 

Ri=inv(Rfilter_MPA); 
elseif  i==5 

Ri=inv(Rfilter_MPA); 
elseif  i==6 

Ri=inv(Rfilter_PC  G) ; 
elseif  i==7 

Ri=inv(Rfrlter_PCG); 
elseif  i==8 

Ri=inv(Rfilter_P  V) ; 
elseif  i==9 

Ri=inv(Rfilter_MP  AC) ; 
end 

%  Construct  information  terms 
info=FT*Ri*z; 
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Info=H'*Ri*H; 

%  Add  the  information  to  prediction 

node.est.y=node.pred.y+info; 

node.est.Y=node.pred.Y+Info; 

%  Seems  to  require  the  next  step  for  stability  in  Matlab 
node.est.Y=force_sym(node.est.Y); 
end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  constructs  the  sensor  network  connectivity  topology 
function  net=set_net_topology(net, nodes, time, METHOD); 
globals; 

switch  upper(METHOD) 
case  'FIXED'  %  topology  is  invariant 
return; 

case  'BROADCAST' 

net=ones(NUM_NODES,NUM_NODES); 
for  i=  1  :NUM_NODES 

net(i,i)=0;  %  nodes  don’t  commuincate  with  themselves 
end 

case  'RANDOM' 
net=net;  %  not  implemented  ye 
otherwise 

disp('Warning:  unknown  network  topology  type'); 
net=net  %  default  is  fixed 
end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  A  function  to  "communicate"  information  between  sensor  nodes 
%  In  general  the  philosophy  is  first  to  identify  the  nodes 
%  in  the  communication  neigbourhood,  then  to  compute  a  common 
%  infomiation  term  locally.  Finally  the  complete  estimate  is 
%  communicated  and  subtraction  of  common  terms  is  done  at  the 
%  receiving  node.  This  approach  makes  the  fusion  process  robust  to 
%  changes  in  communication  topology. 

function  node=communicate(Nodes,com_net,node_num) 

globals; 

%  This  node 

node=Nodes(node_num); 

%  Find  the  current  communication  neighbourhood 
num_chan=l; 
for  i=l  :NUM_NODES 
if  com_net(node_num,i)==T 

%  For  connected  nodes,  compute  common  prior  and  estimate 
%  The  difference  is  the  new  infomiation 
node.chan_out(num_chan)=Nodes(i).pred; 

%  chan  in  contains  the  new  observations 
node.chan_in(num_chan).y=Nodes(i).est.y-Nodes(i).pred.y; 
node.chan_in(num_chan).Y=Nodes(i).est.Y-Nodes(i).pred.Y; 
num_chan=num_chan+ 1 ; 
end 
end 

%  Note  the  number  of  channels  for  later 
nodes. num_chan=num_chan- 1 ; 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  assimilates  all  the  information  from  each  sensor  channel. 

%  The  new  information  is  simply  the  incomming  minus  the  outgoing 
%  infonnation,  or  the  update  minus  the  common  information. 

function  node=assimilate(node,time); 

globals; 

%  Initialisation 
locali=node.est.y-node.pred.y; 
localI=node.est.Y-node.pred.Y; 
yc=node.pred.y; 

Yc=node.pred.Y; 

%  Generate  common  prior  using  covariance  intersection 
if  -FULLCON 

for  i=l  :node.num_chans 

[yc,Yc,omega]=ci_common(node.chan_out(i).y,node.chan_out(i).Y,yc,Yc); 

end 

end 

%  Or  through  fully  connected  assumption 

node.est.y=yc+locali; 

node.est.Y=Y  c+locall; 

for  i=l  :node.num_chans 
node.est.y=node.est.y+node.chan_in(i).y; 
node.est.Y=node.est.Y+node.chan_in(i).Y; 
end 

%  Seems  to  require  the  next  step  for  stability  in  Matlab 
node.est.Y=force_sym(node.est.Y); 

%  This  function  performs  the  covariance  intersect  update  of  two  estimates 
%  (a  with  covariance  A;  b  with  covariance  B)  and  an  observation  matrix  H. 

%  The  output  is  the  output  estimate  (c  with  covariance  C)  and  the  value 
%  of  omega  which  minimizes  the  determinant  of  C. 

function  [yc,Yc, omega]  =  ci_common(ya,Ya,yb,Yb) 
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globals; 


H=[l  0  00; 

00  10; 

1  00  0; 

0  0  10]; 

if  Ya(l)~=0  ||  Yb(l)~=0 

f=inline('l/det(omega*Yai+(l -omega)* Ybi)',’omega',  ’Yai’,  Ybi'); 
Yai=inv(Ya); 

Ybi=inv(Yb); 

YbiH=H'*Ybi; 

YbiHH=YbiH*H; 

omega  =  fminsearch(f,0.5,[],Yai,Ybi); 

Y  ci=Y  ai*omega+YbiHH*(  1  -omega); 

Yc=inv(Yci);  %  New  covariance 

yc=Yc*(Yai*ya+YbiH*yb);  %  New  mean 

else 

Y  c=zeros(XS  IZE,XS  IZE); 
yc=zeros(XSIZE,  1 ); 
omega  =  0; 

end 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  displays  true  target  trajectories,  platform  trajectories, 

%  target  distinations,  and  sensor  observations  following  a  simulation  run 

function  [true, plat, obs,dest]=post_sim(targets, observations, platforms, destinations) 

globals; 

%  Set  plotting  size 

[nsamps,nplatforms]=size(platforms); 

[nsamps,ndestinations]=size(destinations); 

[nsamp  s  ,ntarget  s] =size(targets) ; 

[nsensors,nsamps]=size(observations); 

figure)  1) 
elf; 

hold  on 

data=zeros(2,nsamps); 
title('True  Target  Motions') 
xlabel('x -position  (m)') 
ylabel(’y-position  (m)’) 

%  Plot  true  target  trajectories 
for  n=l:ntargets 
for  i=l:nsamps 
data)  1  ,i)=targets(i,n).x; 
data(2,i)=targets(i,n).y; 
end 

true=plot(data(  1 , : ), data(2 , : ) ,  'b -') ; 
end 

%  Plot  true  platfonu  trajectories 
for  n=l  mplatforms 
for  i=l:nsamps 
data(l  ,i)=platforms(i,n).x; 
data(2,i)=platforms(i,n).y; 
end 

if  n  ==  1 

p  lat=p  lo  t(  data(l,:),data(2,:),’ko'); 
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elseif  n  ==  2 

plat=plot(data(l,:),data(2,:),'ko'); 
elseif  n  ==  3 

plat=plot(data(l,:),data(2,:),'ko'); 
elseif  n  ==  4 

plat=plot(data(l,:),data(2,:),'ko'); 
elseif  n  ==  5 

plat=plot(data(l,:),data(2,:),’ko’); 
elseif  n  ==  6 

plat=plot(data(l,:),data(2,:),’r*'); 
elseif  n  ==  7 

plat=plot(data(l,:),data(2,:),’r*'); 
elseif  n  ==  8 

plat=plot(data(l,:),data(2,:),'gh’); 
elseif  n  ==  9 

plat=plot(data(l,:),data(2,:),’bo’); 

end 

end 

%  Plot  true  destination  locations 
for  n=l  rndestinations 
for  i=l:nsamps 
data(  1  ,i)=destinations(i,n).x; 
data(2,i)=destinations(i,n).y; 
end 

dest=plot(data(l,:),data(2,:),'rh’); 

end 

%  Plot  sensor  observations 
for  n=l:nsensors  %  for  all  sensors 
for  i=l  :nsamps  %  for  all  time 
report=observations(n,i).report; 
for  m=l  mtargets  %  for  all  targets 
if  report(m,l)  >  0  %  if  they  are  seen 

[zx,zy,R]=xy_obs(report,m,n);  %  convert  observation  to  global  xy  coordinates 
odata(l,m)=zx; 
odata(2,m)=zy; 
end 
end 

if  n  ==  1 

obs=plot(odata(l,:),odata(2,:),’k.'); 
elseif  n  ==  2 

obs=plot(odata(l,:),odata(2,:),'k.'); 
elseif  n  ==  3 

obs=plot(odata(l,:),odata(2,:),’k.'); 
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elseif  n  ==  4 

obs=plot(odata(l,:),odata(2,:),’k.'); 
elseif  n  ==  5 

obs=plot(odata(l,:),odata(2,:),’k.'); 
elseif  n  ==  6 

obs=plot(odata(l,:),odata(2,:),’r.'); 
elseif  n  ==  7 

obs=plot(odata(l,:),odata(2,:),'r.'); 
elseif  n  ==  8 

obs=plot(odata(l,:),odata(2,:),’g.'); 
elseif  n  ==  9 

obs=plot(odata(l,:),odata(2,:),'b.'); 

end 

end 

end 

legend([true, plat, obs,dest], 'Target  True  Position','Tracking 

Observations','Intended  Target  Destination') 
hold  off 


Stations',’Target 


391 


%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  extract  an  xy  observation  from  a  range-bearing  report 

function  [zx,zy,R]=xy_obs(rep,n,sensor_id) 

globals; 

sr=sin(rep(n,3)); 
cr=cos(rep(n,3)); 
zx=rep(n,4)+rep(n,2)  *  cr; 
zy=rep(n,  5  )+rep(n,2)  *  sr ; 

ROT=[cr  -sr;  sr  cr]; 
rangc2=rep(n,2)*rep(n,2); 
if  sensor_id==l 

sigma=  [ S IGM A_RAN GE_MP AA2  0;0  range2*SIGMA_BEARING_MPAA2]; 
elseif  sensor_id==2 

sigma=  [ S IGM A_RAN GE_MP AA2  0;0  range2 * SIGMA_BEARIN G_MP AA2] ; 
elseif  sensor_id==3 

sigma=  [ S IGM A_RAN GE_MP A A2  0;0  range2*SIGMA_BEARING_MPAA2]; 
elseif  sensor_id==4 

sigma=  [ S IGM A_RAN GE_MP AA2  0;0  range2 * SIGMA_BEARIN G_MP AA2] ; 
elseif  sensor_id==5 

sigma=  [ S IGM A_RAN GE_MP A A2  0;0  range2*SIGMA_BEARING_MPAA2]; 
elseif  sensor_id==6 

sigma=[SIGMA_RANGE_PCGA2  0;0  range2 * SIGMA_BEARING_PCGA2] ; 
elseif  sensor_id==7 

sigma=[SIGMA_RANGE_PCGA2  0;0  range2*SIGMA_BEARING_PCGA2]; 
elseif  sensor_id==8 

sigma=  [ S IGM A_RAN GE_P VA2  0;0  range2*SIGMA_BEARING_PVA2]; 
elseif  sensor_id==9 

sigma=  [ S IGM A_RAN GE_MP AC A2  0;0  range2 * S IGM A_BE ARIN G_MP AC A2 ] ; 
end 

R=ROT*sigma*ROT’; 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  displays  the  fused  target  track  following  a  simulation  run 
function  plot_tracks(targets,y,Y,yp,Yp,  ty,  TY,typ,TYp,  times) 
globals; 

%  Set  plotting  size 

[ntimes,nsensors,xdim  1  ]=size(y); 

[ntimes,nsensors,xdim2,xdim3]=size(Y); 

odata=zeros(2,ntimes); 

tdata=zeros(2,ntimes); 

title('Target  Motions') 

xlabel('x-position') 

ylabel(’y-position’) 

xlim([250  750]) 

ylim(  [280  440]) 

hold  on 

warning  off; 

%  Estimated  individual  trajectories 
for  s=l:nsensors 
for  i=l:ntimes 

P=inv(squeeze(Y(i,s, :,:))); 
x=P*squeeze(y(i,s,:)); 
odata(l,i,s)=x(l); 
odata(2,i,s)=x(3); 
end 
if  s==l 

plot(odata(l,:,l),odata(2,:,l),'r— ') 
elseif  s==2 

plot(odata(  1 , :  ,2),odata(2,:  ,2),'g— ' ') 
elseif  s==3 

plot(odata(l,:,3),odata(2,:,3),’b— ’) 
elseif  s==4 

plot(odata(  1 , :  ,4),odata(2, :  ,4),'m— ') 
elseif  s==5 

plot(odata(l,:,5),odata(2,:,5),’k— ') 
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elseif  s==6 

plot(odata(l,:,6),odata(2,:,6),’r:’) 
elseif  s==7 

plot(odata(l,:,7),odata(2,:,7),'g:') 
elseif  s==8 

plot(odata(  1 , : ,  8),odata(2, : ,  8),’b : ') 
elseif  s==9 

plot(odata(l,:,9),odata(2,:,9),'m:') 

end 

end 

%  Estimated  fused  trajectory 
for  i=l:ntimes 
x=ty(i,:); 
tdatai  1  ,i)=x(  1 ); 
tdata(2,i)=x(3); 
end 

plot(tdata(  1 ,  1  ),tdata(2, : ,  1  ),’g-') 
hold  off 
warning  on; 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  Modified  by  J.  Ng  on  30  May  2011. 

%  This  function  produces  various  plots  with  regards  to  the  SAW  parameters 
function  plot_errors(targets,y,Y,yp,Yp, times, nfigure, trackstarttime) 
globals; 

%  Initialise 

[ntimes,nsensors,xdim  1  ]=size(y); 

[ntimes,nsensors,xdim2,xdim3]=size(Y); 
data=zeros(  1 0,ntimes); 
pdata=zeros(10,ntimes);  %predicted  data 
edata=zeros(10,ntimes);  %estimated  data 
tdata=zeros(10,ntimes);  %true  data 
esttimesigma=zeros(  1  ,ntimes); 
ntarget  =  NUMTARGETS; 
xmin  =  4500; 
xmax  =  MAXTIME; 

warning  off; 

%  Calculating  and  storing  the  data  for  each  time-step 
for  i=l:ntimes 
P=squeeze(  Y  (i, : , :)); 

x=y(i,0; 

Pp=squeeze(  Yp(i, 

xp=yp(i,0; 

data(  1  ,i)=times(i); 

data(2,i)=(x(l)-targets(i,  ntarget).  x)*  1 80; 
data(3,i)=(x(3)-targets(i,  ntarget).  y)*  1 80; 
sigma=real(sqrt(P)); 
data(4,i)=sigma(l,l)*180; 
data(5,i)=sigma(3,3)*  1 80; 

data(6,i)=(real(sqrt(x(2)*x(2)+x(4)*x(4)))-targets(i,ntarget).vel)*180; 
data(7,i)=atan2(x(4),x(2))-targets(i,ntarget).phi; 
if  abs(data(7,i))  >  pi 
data(7,i)  =  2*pi-abs(data(7,i)); 
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end 

data(8,i)=sqrt(sigma(l,l)*sigma(l,l)+sigma(3,3)*sigma(3,3))*180; 
data(9,i)=sqrt(sigma(2,2)*sigma(2,2)+sigma(4,4)*sigma(4,4))*180; 
data(10,i)=real(sqrt(((x(2)/(x(2)A2  +  x(4)A2))A2)*(sigma(4,4)A2)... 
+((x(4)/(x(2)A2  +  x(4)A2))A2)*(sigma(2,2)A2))); 

tdata(  1  ,i)=times(i); 
tdata(2,i)=targets(i,  l).x*  1 80; 
tdata(3 ,  i)=targets(i,  1 ) .  y  *  1 8  0 ; 
tdata(6,i)=targets(i,  l).vel*  1 80; 
tdata(7,i)=targets(i,  l).phi; 

pdata(  1  ,i)=times(i); 
pdata(2,i)=xp(  1)*  1 80; 
pdata(3,i)=xp(3)*180; 
psigma=sqrt(Pp); 
pdata(4,i)=psigma(  1 , 1)*  1 80; 
pdata(5,i)=psigma(3,3)*180; 
pdata(6,i)=sqrt(xp(2)*xp(2)+xp(4)*xp(4))*  1 80; 
pdata(7,i)=atan2(xp(4),xp(2)); 
if  abs(pdata(7,i))  >  pi 
pdata(7,i)  =  2*pi-abs(pdata(7,i)); 
end 

pdata(8,i)=sqrt(psigma(l,l)*psigma(l,l)+psigma(3,3)*psigma(3,3))*180; 
pdata(9,i)=sqrt(psigma(2,2)*psigma(2,2)+psigma(4,4)*psigma(4,4))*180; 
pdata(  1 0,i)=sqrt((((xp(2))/ (xp(2)  A2+xp(4)A2))A2  *  (psigma(4,4)  *psigma(4,4))) . . . 
+(((xp(4))/ (xp(2)A2+xp(4)A2))A2  *  (psigma(4,4)*psigma(2 ,2)))); 

edata(  1  ,i)=times(i); 
edata(2,i)=x(l)*180; 
edata(3,i)=x(3)*180; 
esigma=real(sqrt(P)); 
edata(4,i)=esigma(  1 , 1)*  1 80; 

edata(5,i)=esigma(3,3)*180; 

edata(6,i)=(real(sqrt(x(2)*x(2)+x(4)*x(4))))* 1 80; 
edata(7,i)=atan2(x(4),x(2)); 
if  abs(edata(7,i))  >  pi 
edata(7,i)  =  2*pi-abs(edata(7,i)); 
end 

edata(8  ,i)=sqrt(esigma(  1,1)*  esigma(  1 , 1  )+esigma(3 ,3)  *  esigma(3 ,3  ))  *  1 80 ; 
edata(9,i)=sqrt(esigma(2,2)*esigma(2,2)+esigma(4,4)*esigma(4,4))*180; 
edata(10,i)=real(sqrt(((x(2)/(x(2)A2  +  x(4)A2))A2)*(esigma(4,4)A2)... 
+((x(4)/(x(2)A2  +  x(4)A2))A2)*(esigma(2,2)A2))); 

esttimesigma(  1  ,i)=sqrt(edata(8,i)A2+. . . 


396 


(kdata(6,i)A2)*(edata(9,i)A2))/... 

edata(6,i); 

end 

%  merging  the  heading  standard  deviations  for  plotting 
p=l;q=l;i=l; 
for  j = 1 :  (ntimes)  *  2 
if  j/2-round(j/2)==0 
combtimedata(l,j)=  times(i); 
combdata(  8  ,j  )=edata(  8  ,p) ; 
combdata(9,j)=edata(9,p); 
combdata(  1 0,j)=edata(  1 0,p); 
p=p+l; 

i=i+l; 

else 

combtimedata(l,j)=  times(i); 
combdata(8,j)=pdata(8,q); 
combdata(9,j)=pdata(9,q); 
combdata(  1 0,j)=pdata(  1 0,q); 
q=q+l; 
end 
end 

%  Plot 

figure(nfigure); 

elf; 

plot(data(l,:),abs(data(2,:)),'b’,data(l,:),data(4,:),'r') 
legend('Estimated  Error', 'Standard  Deviation'); 
buf=sprintf('Error  between  X-component  Estimate  and  Truth'); 
title(buf) 

xlim([xmin  xmax]) 
axis  'auto  y' 

xlabel('Simulated  Time  (s)') 
ylabel('X  error  (m)') 

figure(nfigure+ 1 ); 
elf; 

plot(data(T,:),abs(data(3,:)),’b’,data(l,:),data(5,:),'r') 
legend('Estimated  Error', 'Standard  Deviation’); 
buf=sprintf('Error  between  Y-component  Estimate  and  Truth'); 
title(buf) 

xlim([xmin  xmax]) 
axis  'auto  y' 

xlabel('Simulated  Time  (s)') 
ylabel('Y  error  (m)') 


397 


figure(nfigure+2 ) ; 
elf; 

plot(data(l,:),abs(data(6,:)),’b’,data(l,:),data(9,:),'r') 
legend('Estimated  Error', 'Standard  Deviation’); 
buf=sprintf(’Error  between  Velocity  Estimate  and  Truth’); 
title(buf) 

xlim([xmin  xmax]) 
axis  ’auto  y’ 

xlabel(’Simulated  Time  (s)’) 
ylabel(’Velocity  error  (m/s)’) 

figure(nfigure+3 ) ; 
elf; 

plot(data(T,:),abs(data(7,:)),’b’,data(l,:),data(10,:),'r’) 
legend(’Estimated  Error’, ’Standard  Deviation’); 
buf=sprintf(’Error  between  Heading  Estimate  and  Truth’); 
title(buf) 

xlim([xmin  xmax]) 
axis  ’auto  y’ 

xlabel(’Simulated  Time  (s)’) 
ylabel(’Heading  error  (rads)’) 

figure(nfigure+l  1); 
elf; 

plot(edata(  1  ,:),abs((MAX_TIME-edata(  1 ,:))- 
kdata(6 , : )) ,  ’b  ’,  edata(  1 , : ),  esttimesigma(  l,:),’r’ ) 
legend(’Estimated  Error’, ’Standard  Deviation’); 
buf=sprintf(’Error  between  Time  Estimate  and  Truth’); 
title(buf) 

xlim( [min(kdata(7, 1 000:MAX_TIME/ 1 0))*DT  xmax- 1  *DT]) 
axis  ’auto  y’ 

xlabel(’Simulated  Time  (s)’) 
ylabel(’Time  (s)’) 

warning  on; 
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%  Acknowledgement: 

%  This  code  orginates  from  H.  Durrant-Whyte 
%  Australian  Centre  for  Field  Robotics 
%  The  University  of  Sydney,  Australia 

%  This  function  plots  the  network  nodes  and  communication  links 

function  plot_coms(platforms,com_net) 

globals; 

[nsamps,nplatforms]=size(platforms); 

figure(14) 

elf; 

hold  on 

data=zeros(2,nplatforms); 

ploc=zeros(2,2); 

title('Platforms  and  Communication  Links') 
xlabel('x-position  (m)') 
ylabel('y-position  (m)') 

for  n=l  mplatforms 
data(  1  ,n)=platforms(  1  ,n).x; 
data(2,n)=platforms(  1  ,n).y; 
end 

plat=plot(data(  1  ,:),data(2,:),'ko’); 

for  i=l  mplatforms 
for  j=i:nplatforms 
if  com_net(i,j) 
ploc(l,l)=data(l,i); 
ploc(  1 ,2)=data(  1  ,j); 
ploc(2, 1  )=data(2,i); 
ploc(2,2)=data(2,j); 
end 

links=plot(ploc(l,:),ploc(2,:),'r'); 

end 

end 

legend([plat, links], 'SensorsVCommunication  Links’); 
hold  off 
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